From 37c06e15a9da5400683ed628abe50ee13b71408a Mon Sep 17 00:00:00 2001 From: Will Compton Date: Wed, 5 Nov 2025 15:25:51 -0800 Subject: [PATCH 01/27] refactored unitree_interface with new SDK --- .../hardware/robots/unitree/CMakeLists.txt | 2 +- .../hardware/robots/unitree/g1_interface.h | 209 ++++++++---------- .../hardware/robots/unitree/go2_interface.h | 114 ++++------ .../robots/unitree/unitree_interface.h | 109 ++++----- 4 files changed, 171 insertions(+), 263 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt index 204aed9d..f01a7878 100644 --- a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt +++ b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt @@ -9,7 +9,7 @@ include(FetchContent) FetchContent_Declare( unitree_sdk GIT_REPOSITORY https://github.com/unitreerobotics/unitree_sdk2.git - GIT_TAG 3a4680ae9b00df59e60f7e63cfb0fcc432a9d08d) # main # TODO: Fix the issue here + GIT_TAG 2.0.2) # Pinned to release v2.0.2 set(UNITREE_SDK_BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") set(BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 8774c315..22069290 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -1,8 +1,9 @@ #include "unitree_interface.h" -// Locomotion Client +// Unitree Clients #include #include +#include // IDL #include @@ -28,6 +29,10 @@ namespace obelisk { this->declare_parameter("use_hands", false); use_hands_ = this->get_parameter("use_hands").as_bool(); + if (use_hands_) { + RCLCPP_ERROR_STREAM(this->get_logger(), "use_hands_ = True not supported for g1 interface."); + } + this->declare_parameter("fixed_waist", true); fixed_waist_ = this->get_parameter("fixed_waist").as_bool(); @@ -59,10 +64,13 @@ namespace obelisk { this->declare_parameter>("user_pose", user_pose_); user_pose_ = this->get_parameter("user_pose").as_double_array(); + + // Expose command timeout as a ros parameter + this->declare_parameter("command_timeout", 2.0f); + command_timeout_ = this->get_parameter("command_timeout").as_double(); CMD_TOPIC_ = "rt/lowcmd"; STATE_TOPIC_ = "rt/lowstate"; - ODOM_TOPIC_ = "rt/odommodestate"; MAIN_BOARD_STATE_TOPIC_ = "rt/lf/mainboardstate"; // TODO: Add support for rt/lowstate_doubleimu @@ -82,8 +90,11 @@ namespace obelisk { InitializeLogging(); } - loco_client_.SetTimeout(10.0f); + loco_client_.SetTimeout(command_timeout_); loco_client_.Init(); + motion_switcher_client_ = std::make_shared(); + motion_switcher_client_->SetTimeout(command_timeout_); + motion_switcher_client_->Init(); } protected: void CreateUnitreePublishers() override { @@ -100,9 +111,6 @@ namespace obelisk { lowstate_subscriber_.reset(new ChannelSubscriber(STATE_TOPIC_)); lowstate_subscriber_->InitChannel(std::bind(&G1Interface::LowStateHandler, this, std::placeholders::_1), 10); - // odom_subscriber_.reset(new ChannelSubscriber(HG_ODOM_TOPIC)); - // odom_subscriber_->InitChannel(std::bind(&ObeliskUnitreeInterface::OdomHandler, this, std::placeholders::_1), 10); - main_board_subscriber_.reset(new ChannelSubscriber(MAIN_BOARD_STATE_TOPIC_)); main_board_subscriber_->InitChannel(std::bind(&G1Interface::MainboardHandler, this, std::placeholders::_1), 10); } @@ -110,7 +118,7 @@ namespace obelisk { // TODO: Adjust this function so that we can go to a user pose even when no low level control functions are being sent void ApplyControl(const unitree_control_msg& msg) override { // Only execute of in Low Level Control or Home modes - if (exec_fsm_state_ != ExecFSMState::LOW_LEVEL_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { + if (exec_fsm_state_ != ExecFSMState::USER_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { return; } RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Applying control to Unitree robot!"); @@ -121,7 +129,7 @@ namespace obelisk { dds_low_command.mode_machine() = mode_machine_; // ------------------------ Execution FSM: Low Level Control ------------------------ // - if (exec_fsm_state_ == ExecFSMState::LOW_LEVEL_CTRL) { + if (exec_fsm_state_ == ExecFSMState::USER_CTRL) { // ---------- Verify message ---------- // if (msg.joint_names.size() != msg.pos_target.size() || msg.joint_names.size() != msg.vel_target.size() || msg.joint_names.size() != msg.feed_forward.size()) { RCLCPP_ERROR_STREAM(this->get_logger(), "joint name size: " << msg.joint_names.size()); @@ -190,16 +198,15 @@ namespace obelisk { // ------------------------ Execution FSM: Home ------------------------ // } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { // Compute time that robot has been in HOME - // float t = std::chrono::duration( - // std::chrono::steady_clock::now() - user_pose_transition_start_time_).count(); - // Compute proportion of time relative to transition duration - // TODO this is not working - //float proportion = std::min(t / user_pose_transition_duration_, 1.0f); + float t = std::chrono::duration( + std::chrono::steady_clock::now() - user_pose_transition_start_time_).count(); + // Compute proportion of time relative to transition duration + float proportion = std::min(t / user_pose_transition_duration_, 1.0f); // Write message - for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; i++) { // Sending the extra waist commands while in fixed waist should have no effect + for (size_t i = 0; i < num_motors_; i++) { dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(i).tau() = 0.; - dds_low_command.motor_cmd().at(i).q() = user_pose_[i];//(1 - proportion) * start_user_pose_[i] + proportion * user_pose_[i]; + dds_low_command.motor_cmd().at(i).q() = (1 - proportion) * start_user_pose_[i] + proportion * user_pose_[i]; dds_low_command.motor_cmd().at(i).dq() = 0.; dds_low_command.motor_cmd().at(i).kp() = kp_[i]; dds_low_command.motor_cmd().at(i).kd() = kd_[i]; @@ -284,12 +291,11 @@ namespace obelisk { RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Main board value: " << board_state.value()[0] << ", " << board_state.value()[1] << ", " << board_state.value()[2] << ", " << board_state.value()[3] << ", " << board_state.value()[4] << ", " << board_state.value()[5] ); } - void ApplyHighLevelControl(const unitree_high_level_ctrl_msg& msg) override { - if (exec_fsm_state_ != ExecFSMState::HIGH_LEVEL_CTRL) { + void ApplyVelCmd(const unitree_vel_cmd_msg& msg) override { + if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; } - - loco_client_.SetVelocity(msg.v_x, msg.v_y, msg.w_z); + loco_client_.Move(msg.v_x, msg.v_y, msg.w_z); } bool CheckDampingToUnitreeHomeTransition() { @@ -299,70 +305,50 @@ namespace obelisk { void TransitionToUserPose() override{ // First, save current position + RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM TRANSTION TO user_pose!"); { - std::lock_guard lock(joint_pos_mutex_); - joint_pos_ = start_user_pose_; + std::lock_guard lock(joint_mutex_); + start_user_pose_ = joint_pos_; } user_pose_transition_start_time_ = std::chrono::steady_clock::now(); // Save start time } void TransitionToUnitreeHome() override{ + loco_client_.StopMove(); loco_client_.BalanceStand(); } - // void OdomHandler(const void *message) { - // RCLCPP_INFO_STREAM(this->get_logger(), "IN ODOM"); - // IMUState_ imu_state = *(const IMUState_ *)message; - // // if (imu_state.crc() != Crc32Core((uint32_t *)&imu_state, (sizeof(imu_state) >> 2) - 1)) { - // // RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeRobotInterface] CRC Error"); - // // return; - // // } - - // // IMU - // obelisk_sensor_msgs::msg::ObkImu obk_imu_state; - // obk_imu_state.header.stamp = this->now(); - // obk_imu_state.angular_velocity.x = imu_state.gyroscope()[0]; - // obk_imu_state.angular_velocity.y = imu_state.gyroscope()[1]; - // obk_imu_state.angular_velocity.z = imu_state.gyroscope()[2]; - - // obk_imu_state.orientation.w = imu_state.quaternion()[0]; - // obk_imu_state.orientation.x = imu_state.quaternion()[1]; - // obk_imu_state.orientation.y = imu_state.quaternion()[2]; - // obk_imu_state.orientation.z = imu_state.quaternion()[3]; - - // obk_imu_state.linear_acceleration.x = imu_state.accelerometer()[0]; - // obk_imu_state.linear_acceleration.y= imu_state.accelerometer()[1]; - // obk_imu_state.linear_acceleration.z = imu_state.accelerometer()[2]; - - // this->GetPublisher("odom_state_pub")->publish(obk_imu_state); - // } - - bool use_hands_; - bool fixed_waist_; - - std::string ODOM_TOPIC_; - std::string MAIN_BOARD_STATE_TOPIC_; - - unitree::robot::ChannelPublisherPtr lowcmd_publisher_; - ChannelSubscriberPtr lowstate_subscriber_; - ChannelSubscriberPtr main_board_subscriber_; - // ChannelSubscriberPtr odom_subscriber_; - - - enum class AnkleMode { - PR = 0, // Series Control for Ptich/Roll Joints - AB = 1 // Parallel Control for A/B Joints - }; + void TransitionToDamp() override{ + loco_client_.StopMove(); + loco_client_.Damp(); + } - AnkleMode mode_pr_; - uint8_t mode_machine_; + bool ReleaseUnitreeMotionControl() { + std::string robot_form, motion_mode; + int32_t ret = motion_switcher_client_->CheckMode(robot_form, motion_mode); + if (ret != 0) { + RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); + return false; + } - // Locomotion Client - g1::LocoClient loco_client_; + if (!motion_mode.empty()) { + return motion_switcher_client_->ReleaseMode() == 0; + } + return true; + } - // Logging - std::ofstream motor_data_log_; - rclcpp::Time startup_time_; + bool EngageUnitreeMotionControl() { + std::string robot_form, motion_mode; + int32_t ret = motion_switcher_client_->CheckMode(robot_form, motion_mode); + if (ret != 0) { + RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); + return false; + } + if (motion_mode.empty()) { + return motion_switcher_client_->SelectMode("normal") == 0; + } + return true; + } void InitializeLogging() { // Create and open motor data log file in the existing log directory @@ -424,6 +410,27 @@ namespace obelisk { this->log_count_++; } + bool use_hands_; + bool fixed_waist_; + + std::string MAIN_BOARD_STATE_TOPIC_; + + ChannelSubscriberPtr lowstate_subscriber_; + unitree::robot::ChannelPublisherPtr lowcmd_publisher_; + ChannelSubscriberPtr main_board_subscriber_; + + enum class AnkleMode { + PR = 0, // Series Control for Ptich/Roll Joints + AB = 1 // Parallel Control for A/B Joints + }; + + AnkleMode mode_pr_; + uint8_t mode_machine_; + + // Logging + std::ofstream motor_data_log_; + rclcpp::Time startup_time_; + private: // ---------- Robot Specific ---------- // // G1 @@ -431,19 +438,19 @@ namespace obelisk { static constexpr int G1_EXTRA_WAIST = 2; static constexpr int G1_HAND_MOTOR_NUM = 14; - float user_pose_transition_duration_; // Duration of the transition to home position + float user_pose_transition_duration_; // Duration of the transition to home position + std::chrono::steady_clock::time_point user_pose_transition_start_time_; // Timing variable for transition to stand + float command_timeout_; // How long to wait before timing out unitree commands. + + std::vector joint_pos_; // Local copy of joint positions + std::vector joint_vel_; // Local copy of joint positions + std::vector start_user_pose_; // For transitioning to home position + std::vector user_pose_; // home position - std::vector joint_pos_; // Local copy of joint positions - std::vector joint_vel_; // Local copy of joint positions - std::vector start_user_pose_; // For transitioning to home position - std::vector user_pose_; // home position - // float joint_pos_[G1_MOTOR_NUM + G1_HAND_MOTOR_NUM]; - // float joint_vel_[G1_MOTOR_NUM + G1_HAND_MOTOR_NUM]; // Local copy of joint velocities - // float start_user_pose_[G1_MOTOR_NUM + G1_HAND_MOTOR_NUM]; // For transitioning to home position - // float user_pose_[G1_MOTOR_NUM + G1_HAND_MOTOR_NUM]; // home position + std::mutex joint_mutex_; // mutex for copying joint positions and velocities + g1::LocoClient loco_client_; // Locomotion Client for high level control + std::shared_ptr motion_switcher_client_; // Robot State Client for high level/low level handoff - std::mutex joint_pos_mutex_; // mutex for copying joint positions and velocities - std::chrono::steady_clock::time_point user_pose_transition_start_time_; // Timing variable for transition to stand std::vector default_user_pose_ = { 0.3, 0, 0, 0.52, -0.23, 0, // Left Leg -0.3, 0, 0, 0.52, -0.23, 0, // Right Leg @@ -552,44 +559,6 @@ namespace obelisk { "right_hand_index_1_joint" }; - // enum G1JointIndex { - // LeftHipPitch = 0, - // LeftHipRoll = 1, - // LeftHipYaw = 2, - // LeftKnee = 3, - // LeftAnklePitch = 4, - // LeftAnkleB = 4, - // LeftAnkleRoll = 5, - // LeftAnkleA = 5, - // RightHipPitch = 6, - // RightHipRoll = 7, - // RightHipYaw = 8, - // RightKnee = 9, - // RightAnklePitch = 10, - // RightAnkleB = 10, - // RightAnkleRoll = 11, - // RightAnkleA = 11, - // WaistYaw = 12, - // WaistRoll = 13, // NOTE INVALID for g1 23dof/29dof with waist locked - // WaistA = 13, // NOTE INVALID for g1 23dof/29dof with waist locked - // WaistPitch = 14, // NOTE INVALID for g1 23dof/29dof with waist locked - // WaistB = 14, // NOTE INVALID for g1 23dof/29dof with waist locked - // LeftShoulderPitch = 15, - // LeftShoulderRoll = 16, - // LeftShoulderYaw = 17, - // LeftElbow = 18, - // LeftWristRoll = 19, - // LeftWristPitch = 20, // NOTE INVALID for g1 23dof - // LeftWristYaw = 21, // NOTE INVALID for g1 23dof - // RightShoulderPitch = 22, - // RightShoulderRoll = 23, - // RightShoulderYaw = 24, - // RightElbow = 25, - // RightWristRoll = 26, - // RightWristPitch = 27, // NOTE INVALID for g1 23dof - // RightWristYaw = 28 // NOTE INVALID for g1 23dof - // }; - const std::map G1_JOINT_MAPPINGS = { {"left_hip_pitch_joint", 0}, {"left_hip_roll_joint", 1}, diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 3a107ded..5d59fe89 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -1,9 +1,8 @@ #include "unitree_interface.h" -#include "obelisk_sensor_msgs/msg/obk_force_sensor.hpp" - -// Sport Client +// Unitree Clients #include +#include // IDL #include @@ -18,9 +17,6 @@ namespace obelisk { Go2Interface(const std::string& node_name) : ObeliskUnitreeInterface(node_name) { - // Additional Publishers - this->RegisterObkPublisher("pub_force_sensor_setting", "force_sensor_key"); - // Expose duration of transition to home position as ros parameter this->declare_parameter("user_pose_transition_duration", 1.); user_pose_transition_duration_ = this->get_parameter("user_pose_transition_duration").as_double(); @@ -34,6 +30,10 @@ namespace obelisk { auto user_pose_vector = this->get_parameter("home_position").as_double_array(); // Get as vector std::copy(user_pose_vector.begin(), user_pose_vector.end(), user_pose_); + // Expose command timeout as a ros parameter + this->declare_parameter("command_timeout", 2.0f); + command_timeout_ = this->get_parameter("command_timeout").as_double(); + // Handle joint names num_motors_ = GO2_MOTOR_NUM; for (size_t i = 0; i < num_motors_; i++) { @@ -43,15 +43,16 @@ namespace obelisk { // Define topic names CMD_TOPIC_ = "rt/lowcmd"; STATE_TOPIC_ = "rt/lowstate"; - // TODO: Look for ODOM topic // Verify params and create unitree publishers VerifyParameters(); CreateUnitreePublishers(); // Initialize sports client - sport_client_.SetTimeout(10.0f); + sport_client_.SetTimeout(command_timeout_); sport_client_.Init(); + robot_state_client_.SetTimeout(command_timeout_); + robot_state_client_.Init(); } protected: void CreateUnitreePublishers() override { @@ -67,13 +68,11 @@ namespace obelisk { // Need to create after the publishers have been activated lowstate_subscriber_.reset(new ChannelSubscriber(STATE_TOPIC_)); lowstate_subscriber_->InitChannel(std::bind(&Go2Interface::LowStateHandler, this, std::placeholders::_1), 1); - - // Odom state? } void ApplyControl(const unitree_control_msg& msg) override { // Only execute of in Low Level Control or Home modes - if (exec_fsm_state_ != ExecFSMState::LOW_LEVEL_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { + if (exec_fsm_state_ != ExecFSMState::USER_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { return; } RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Applying control to Unitree robot!"); @@ -82,7 +81,7 @@ namespace obelisk { LowCmd_ dds_low_command; // ------------------------ Execution FSM: Low Level Control ------------------------ // - if (exec_fsm_state_ == ExecFSMState::LOW_LEVEL_CTRL) { + if (exec_fsm_state_ == ExecFSMState::USER_CTRL) { // ---------- Verify message ---------- // if (msg.joint_names.size() != msg.pos_target.size() || msg.joint_names.size() != msg.vel_target.size() || msg.joint_names.size() != msg.feed_forward.size()) { RCLCPP_ERROR_STREAM(this->get_logger(), "joint name size: " << msg.joint_names.size()); @@ -184,7 +183,7 @@ namespace obelisk { joint_state.joint_pos.at(i) = low_state.motor_state()[i].q(); joint_state.joint_vel.at(i) = low_state.motor_state()[i].dq(); { - std::lock_guard lock(joint_pos_mutex_); + std::lock_guard lock(joint_mutex_); joint_pos_[i] = low_state.motor_state()[i].q(); joint_vel_[i] = low_state.motor_state()[i].dq(); } @@ -216,38 +215,14 @@ namespace obelisk { imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2]; this->GetPublisher(pub_imu_state_key_)->publish(imu_state); - - // Foot forces - obelisk_sensor_msgs::msg::ObkForceSensor force_sensor; - force_sensor.header.stamp = this->now(); - force_sensor.forces.resize(4); - force_sensor.frames = {"FR_foot", "FL_foot", "RR_foot", "RL_foot"}; - for (int i = 0; i < 4; i++) { - force_sensor.forces[i] = low_state.foot_force()[i]; - } - this->GetPublisher("force_sensor_key")->publish(force_sensor); } - void ApplyHighLevelControl(const unitree_high_level_ctrl_msg& msg) override { + void ApplyVelCmd(const unitree_vel_cmd_msg& msg) override { // Ignore if not in high-level mode - if (exec_fsm_state_ != ExecFSMState::HIGH_LEVEL_CTRL) { + if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; } - - // Include logic to stand when velocity is within deadzone - static bool standing = false; - if (msg.v_x * msg.v_x + msg.v_y * msg.v_y + msg.w_z * msg.w_z > vel_deadzone_) { - if (standing) { - sport_client_.SwitchGait(1); // Set gait to trot - standing = false; - } - sport_client_.Move(msg.v_x, msg.v_y, msg.w_z); // Command velocity - } else { - if (!standing) { - sport_client_.SwitchGait(0); // Set gait to stand - standing = true; - } - } + sport_client_.Move(msg.v_x, msg.v_y, msg.w_z); // Command velocity } bool CheckDampingToUnitreeHomeTransition() { @@ -267,7 +242,7 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM TRANSTION TO user_pose!"); // First, save current position { - std::lock_guard lock(joint_pos_mutex_); + std::lock_guard lock(joint_mutex_); std::copy(std::begin(joint_pos_), std::end(joint_pos_), start_user_pose_); } user_pose_transition_start_time_ = std::chrono::steady_clock::now(); // Save start time @@ -277,54 +252,43 @@ namespace obelisk { sport_client_.BalanceStand(); } - // void OdomHandler(const void *message) { - // RCLCPP_INFO_STREAM(this->get_logger(), "IN ODOM"); - // IMUState_ imu_state = *(const IMUState_ *)message; - // // if (imu_state.crc() != Crc32Core((uint32_t *)&imu_state, (sizeof(imu_state) >> 2) - 1)) { - // // RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeRobotInterface] CRC Error"); - // // return; - // // } - - // // IMU - // obelisk_sensor_msgs::msg::ObkImu obk_imu_state; - // obk_imu_state.header.stamp = this->now(); - // obk_imu_state.angular_velocity.x = imu_state.gyroscope()[0]; - // obk_imu_state.angular_velocity.y = imu_state.gyroscope()[1]; - // obk_imu_state.angular_velocity.z = imu_state.gyroscope()[2]; - - // obk_imu_state.orientation.w = imu_state.quaternion()[0]; - // obk_imu_state.orientation.x = imu_state.quaternion()[1]; - // obk_imu_state.orientation.y = imu_state.quaternion()[2]; - // obk_imu_state.orientation.z = imu_state.quaternion()[3]; - - // obk_imu_state.linear_acceleration.x = imu_state.accelerometer()[0]; - // obk_imu_state.linear_acceleration.y= imu_state.accelerometer()[1]; - // obk_imu_state.linear_acceleration.z = imu_state.accelerometer()[2]; + void TransitionToDamp() override{ + sport_client_.StopMove(); + sport_client_.Damp(); + } - // this->GetPublisher("odom_state_pub")->publish(obk_imu_state); - // } + bool EngageUnitreeMotionControl() override{ + int32_t ret; + robot_state_client_.ServiceSwitch("mcf", 1, ret); // Turn the Unitree motion control on + return ret == 0; + } - std::string ODOM_TOPIC_; + bool ReleaseUnitreeMotionControl() override{ + int32_t ret; + robot_state_client_.ServiceSwitch("mcf", 0, ret); // Turn the Unitree motion control off + return ret == 0; + } unitree::robot::ChannelPublisherPtr lowcmd_publisher_; ChannelSubscriberPtr lowstate_subscriber_; - // ChannelSubscriberPtr odom_subscriber_; private: static constexpr int GO2_MOTOR_NUM = 12; // Number of motors float joint_pos_[GO2_MOTOR_NUM]; // Local copy of joint positions float joint_vel_[GO2_MOTOR_NUM]; // Local copy of joint velocities - float start_user_pose_[GO2_MOTOR_NUM]; // For transitioning to home position - float user_pose_transition_duration_; // Duration of the transition to home position + float start_user_pose_[GO2_MOTOR_NUM]; // For transitioning to home position + float user_pose_transition_duration_; // Duration of the transition to home position float vel_deadzone_; // Deadzone for high level velocity commands + float command_timeout_; // How long to wait before timing out unitree commands. std::vector default_user_pose_ = {0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8}; // Default home position - float user_pose_[GO2_MOTOR_NUM]; // home position - std::mutex joint_pos_mutex_; // mutex for copying joint positions and velocities - go2::SportClient sport_client_; // Sports client for high level control - std::chrono::steady_clock::time_point user_pose_transition_start_time_; // Timing variable for transition to stand - const std::array GO2_JOINT_NAMES = { // Names of joints, in order for hardware + float user_pose_[GO2_MOTOR_NUM]; // home position + std::mutex joint_mutex_; // mutex for copying joint positions and velocities + go2::SportClient sport_client_; // Sports client for high level control + go2::RobotStateClient robot_state_client_; // Robot State Client for high level/low level control handoff + std::chrono::steady_clock::time_point user_pose_transition_start_time_; // Timing variable for transition to stand + const std::array GO2_JOINT_NAMES = { // Names of joints, in order for hardware "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index f3564562..ec5d054b 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -17,12 +17,12 @@ #include // IDL -#include +#include // Get rid of this?? namespace obelisk { using unitree_control_msg = obelisk_control_msgs::msg::PDFeedForward; using unitree_fsm_msg = obelisk_control_msgs::msg::ExecutionFSM; - using unitree_high_level_ctrl_msg = obelisk_control_msgs::msg::VelocityCommand; + using unitree_vel_cmd_msg = obelisk_control_msgs::msg::VelocityCommand; using namespace unitree::common; using namespace unitree::robot; @@ -73,8 +73,8 @@ namespace obelisk { this->RegisterObkSubscription( "sub_fsm_setting", sub_fsm_key_, std::bind(&ObeliskUnitreeInterface::TransitionFSM, this, std::placeholders::_1)); // Register High Level Control Subscriber - this->RegisterObkSubscription( - "sub_high_level_ctrl_setting", sub_high_level_ctrl_key_, std::bind(&ObeliskUnitreeInterface::ApplyHighLevelControl, this, std::placeholders::_1)); + this->RegisterObkSubscription( + "sub_vel_cmd_setting", sub_UNITREE_VEL_CTRL_key_, std::bind(&ObeliskUnitreeInterface::ApplyVelCmd, this, std::placeholders::_1)); // ---------- Default PD gains ---------- // this->declare_parameter>("default_kp"); @@ -83,13 +83,12 @@ namespace obelisk { kd_ = this->get_parameter("default_kd").as_double_array(); // TODO: User defined safety maybe - ChannelFactory::Instance()->Init(0, network_interface_name_); // Try to shutdown motion control-related service - mode_switch_manager_ = std::make_shared(); - mode_switch_manager_->SetTimeout(5.0f); - mode_switch_manager_->Init(); + // mode_switch_manager_ = std::make_shared(); + // mode_switch_manager_->SetTimeout(5.0f); + // mode_switch_manager_->Init(); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_activate( @@ -105,47 +104,53 @@ namespace obelisk { INIT = 0, UNITREE_HOME = 1, USER_POSE = 2, - LOW_LEVEL_CTRL = 3, - HIGH_LEVEL_CTRL = 4, - DAMPING = 5 + USER_CTRL = 3, + UNITREE_VEL_CTRL = 4, + DAMPING = 5, + ESTOP = 6 }; const std::unordered_map TRANSITION_STRINGS = { {ExecFSMState::INIT, "INIT"}, {ExecFSMState::UNITREE_HOME, "UNITREE_HOME"}, {ExecFSMState::USER_POSE, "USER_POSE"}, - {ExecFSMState::LOW_LEVEL_CTRL, "LOW_LEVEL_CONTROL"}, - {ExecFSMState::HIGH_LEVEL_CTRL, "HIGH_LEVEL_CONTROL"}, - {ExecFSMState::DAMPING, "DAMPING"} + {ExecFSMState::USER_CTRL, "LOW_LEVEL_CONTROL"}, + {ExecFSMState::UNITREE_VEL_CTRL, "HIGH_LEVEL_CONTROL"}, + {ExecFSMState::DAMPING, "DAMPING"}, + {ExecFSMState::ESTOP, "ESTOP"} }; // Set of legal transitions for the execution FSM const std::unordered_map> TRANSITIONS = { - {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING}}, // Init -> Home, Init -> Damp - {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::LOW_LEVEL_CTRL, ExecFSMState::HIGH_LEVEL_CTRL, ExecFSMState::DAMPING}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp - {ExecFSMState::USER_POSE, {ExecFSMState::UNITREE_HOME, ExecFSMState::LOW_LEVEL_CTRL, ExecFSMState::DAMPING}}, // Pose -> Home, Pose -> Low, Pose -> Damp - {ExecFSMState::LOW_LEVEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::DAMPING}}, // Low -> Home, Low -> Pose, Low -> Damp - {ExecFSMState::HIGH_LEVEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING}}, // High -> Home, High -> Damp - {ExecFSMState::DAMPING, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE}} // Damp -> Home, Damp -> Pose + {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Init -> Home, Init -> Damp + {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::UNITREE_VEL_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp + {ExecFSMState::USER_POSE, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Pose -> Home, Pose -> Low, Pose -> Damp + {ExecFSMState::USER_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Low -> Home, Low -> Pose, Low -> Damp + {ExecFSMState::UNITREE_VEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // High -> Home, High -> Damp + {ExecFSMState::DAMPING, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::ESTOP}}, // Damp -> Home, Damp -> Pose + {ExecFSMState::ESTOP, {}} // Estop (None) }; // Set of transitions on which motion control must be released const std::vector RELEASE_MC_STATES = { - ExecFSMState::USER_POSE, ExecFSMState::LOW_LEVEL_CTRL, ExecFSMState::DAMPING + ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP }; // Set of transitions on which motion control must be engaged const std::vector ENGAGE_MC_STATES = { - ExecFSMState::UNITREE_HOME, ExecFSMState::HIGH_LEVEL_CTRL + ExecFSMState::UNITREE_HOME, ExecFSMState::UNITREE_VEL_CTRL }; protected: virtual void CreateUnitreeSubscribers() = 0; virtual void CreateUnitreePublishers() = 0; - virtual void ApplyHighLevelControl(const unitree_high_level_ctrl_msg& msg) = 0; + virtual void ApplyVelCmd(const unitree_vel_cmd_msg& msg) = 0; virtual bool CheckDampingToUnitreeHomeTransition() = 0; virtual void TransitionToUnitreeHome() = 0; virtual void TransitionToUserPose() = 0; + virtual void TransitionToDamp() = 0; + virtual bool EngageUnitreeMotionControl() = 0; + virtual bool ReleaseUnitreeMotionControl() = 0; void TransitionFSM(const unitree_fsm_msg& msg) { // Extract commanded FSM state from the message @@ -161,29 +166,35 @@ namespace obelisk { // Under some transitions, need to release the motion control if (std::find(RELEASE_MC_STATES.begin(), RELEASE_MC_STATES.end(), cmd_exec_fsm_state) != RELEASE_MC_STATES.end()) { - bool success = ReleaseMotionControl(); + bool success = ReleaseUnitreeMotionControl(); if (!success) { - RCLCPP_ERROR_STREAM(this->get_logger(), "RELEASING MOTION CONTROL FAILED!"); + RCLCPP_ERROR_STREAM(this->get_logger(), "RELEASING UNITREE MOTION CONTROL FAILED!"); return; } - RCLCPP_INFO_STREAM(this->get_logger(), "RELEASED MOTION CONTROL!"); + RCLCPP_INFO_STREAM(this->get_logger(), "RELEASED UNITREE MOTION CONTROL!"); } // Under some transitions, need to re-engage the motion control if (std::find(ENGAGE_MC_STATES.begin(), ENGAGE_MC_STATES.end(), cmd_exec_fsm_state) != ENGAGE_MC_STATES.end()) { - bool success = EngageMotionControl(); + bool success = EngageUnitreeMotionControl(); if (!success) { - RCLCPP_ERROR_STREAM(this->get_logger(), "ENGAGING MOTION CONTROL FAILED!"); + RCLCPP_ERROR_STREAM(this->get_logger(), "ENGAGING UNITREE MOTION CONTROL FAILED!"); return; } - RCLCPP_INFO_STREAM(this->get_logger(), "ENGAGED MOTION CONTROL!"); + RCLCPP_INFO_STREAM(this->get_logger(), "ENGAGED UNITREE MOTION CONTROL!"); } // Transition the FSM exec_fsm_state_ = cmd_exec_fsm_state; RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM STATE TRANSITIONED TO " << TRANSITION_STRINGS.at(exec_fsm_state_)); - if (exec_fsm_state_ == ExecFSMState::UNITREE_HOME) { + if (exec_fsm_state_ == ExecFSMState::ESTOP) { + RCLCPP_ERROR_STREAM(this->get_logger(), "ESTOP TRIGGERED"); + TransitionToDamp(); + throw std::runtime_error("ESTOP TRIGGERED"); + } else if (exec_fsm_state_ == ExecFSMState::DAMPING) { + TransitionToDamp(); + } else if (exec_fsm_state_ == ExecFSMState::UNITREE_HOME) { TransitionToUnitreeHome(); } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { TransitionToUserPose(); @@ -250,42 +261,6 @@ namespace obelisk { return std::find(transitions[state].begin(), transitions[state].end(), next_state) != transitions[state].end(); } - // bool ReleaseMotionControl() { - // int32_t ret = mode_switch_manager_->ReleaseMode(); - // return ret == 0; - // } - - bool ReleaseMotionControl() { - std::string robot_form, motion_mode; - int ret = mode_switch_manager_->CheckMode(robot_form, motion_mode); - if (ret == 0) { - RCLCPP_INFO_STREAM(this->get_logger(), "[UnitreeInterface] Check mode succeeded."); - } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); - return false; - } - - if (!motion_mode.empty()) { - return mode_switch_manager_->ReleaseMode() == 0; - } - return true; - } - - // bool EngageMotionControl() { - // // TODO: What goes here?? - // int32_t ret = mode_switch_manager_->SelectMode("normal"); // Can be "normal", "ai", or "advanced" ?? From reading example code. - // return ret == 0; - // } - - bool EngageMotionControl() { - std::string robot_form, motion_mode; - mode_switch_manager_->CheckMode(robot_form, motion_mode); - if (motion_mode.empty()) { - return mode_switch_manager_->SelectMode("normal") == 0; - } - return true; - } - // Execution FSM state variable ExecFSMState exec_fsm_state_; @@ -306,7 +281,7 @@ namespace obelisk { // Keys const std::string sub_fsm_key_ = "sub_exec_fsm_key"; - const std::string sub_high_level_ctrl_key_ = "sub_high_level_ctrl_key"; + const std::string sub_UNITREE_VEL_CTRL_key_ = "sub_vel_cmd_key"; const std::string pub_joint_state_key_ = "joint_state_pub"; const std::string pub_imu_state_key_ = "imu_state_pub"; From b97ce1288b38e8fe01c108ba270c9998f5ab9c96 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Wed, 5 Nov 2025 17:31:45 -0800 Subject: [PATCH 02/27] wip joystick modifications --- .../cpp/hardware/include/unitree_joystick.h | 337 ++++++++++++++---- 1 file changed, 272 insertions(+), 65 deletions(-) diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index 6897e24e..a95e194c 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -4,101 +4,218 @@ #include "obelisk_control_msgs/msg/execution_fsm.hpp" #include "obelisk_control_msgs/msg/velocity_command.hpp" + namespace obelisk { using unitree_fsm_msg = obelisk_control_msgs::msg::ExecutionFSM; - using unitree_high_level_ctrl_msg = obelisk_control_msgs::msg::VelocityCommand; + using vel_cmd_msg = obelisk_control_msgs::msg::VelocityCommand; - class UnitreeJoystick : public ObeliskController { + class UnitreeJoystick : public ObeliskController { public: UnitreeJoystick(const std::string& name) - : ObeliskController(name) { + : ObeliskController(name) { // Set velocity bounds - this->declare_parameter("v_x_max", 0.5); - this->declare_parameter("v_y_max", 0.5); - this->declare_parameter("w_z_max", 0.5); - v_x_max_ = this->get_parameter("v_x_max").as_double(); - v_y_max_ = this->get_parameter("v_y_max").as_double(); - w_z_max_ = this->get_parameter("w_z_max").as_double(); + this->declare_parameter("v_x_scale", 0.5); + this->declare_parameter("v_y_scale", 0.5); + this->declare_parameter("w_z_scale", 0.5); + v_x_scale_ = this->get_parameter("v_x_scale").as_double(); + v_y_scale_ = this->get_parameter("v_y_scale").as_double(); + w_z_scale_ = this->get_parameter("w_z_scale").as_double(); // Handle Execution FSM buttons // Buttons with negative values will corrospond to the DPAD - this->declare_parameter("unitree_home_button", -1); - this->declare_parameter("user_pose_button", 6); - this->declare_parameter("low_level_ctrl_button", -1); - this->declare_parameter("high_level_ctrl_button", -1); - this->declare_parameter("damping_button", -1); + this->declare_parameter("unitree_home_button", ButtonMap::DR1); unitree_home_button_ = this->get_parameter("unitree_home_button").as_int(); + unitree_home_button_on_layer_ = isOnLayer(unitree_home_button_); + unitree_home_button_on_axis_ = isOnAxis(unitree_home_button_); + unitree_home_button_on_dpad_ = isOnDpad(unitree_home_button); + if !recognizedButton(unitree_home_button) throw std::runtime_error("[UnitreeJoystick] Unitree Home Button " << unitree_home_button_ << " not recognized!!"); + + this->declare_parameter("user_pose_button", ButtonMap::DL1); user_pose_button_ = this->get_parameter("user_pose_button").as_int(); + user_pose_button_on_layer_ = isOnLayer(user_pose_button_); + user_pose_button_on_axis_ = isOnAxis(user_pose_button_); + user_pose_button_on_dpad_ = isOnDpad(user_pose_button_); + if !recognizedButton(user_pose_button_) throw std::runtime_error("[UnitreeJoystick] Unitree User Pose Button " << user_pose_button_ << " not recognized!!"); + + this->declare_parameter("low_level_ctrl_button", ButtonMap::DD1); low_level_ctrl_button_ = this->get_parameter("low_level_ctrl_button").as_int(); + low_level_ctrl_button_on_layer_ = isOnLayer(low_level_ctrl_button_); + low_level_ctrl_button_on_axis_ = isOnAxis(low_level_ctrl_button_); + low_level_ctrl_button_on_dpad_ = isOnDpad(low_level_ctrl_button_); + if !recognizedButton(low_level_ctrl_button_) throw std::runtime_error("[UnitreeJoystick] Unitree Low Level Control Button " << low_level_ctrl_button_ << " not recognized!!"); + + this->declare_parameter("high_level_ctrl_button", ButtonMap::DU1); high_level_ctrl_button_ = this->get_parameter("high_level_ctrl_button").as_int(); + high_level_ctrl_button_on_layer_ = isOnLayer(high_level_ctrl_button_); + high_level_ctrl_button_on_axis_ = isOnAxis(high_level_ctrl_button_); + high_level_ctrl_button_on_dpad_ = isOnDpad(high_level_ctrl_button_); + if !recognizedButton(high_level_ctrl_button_) throw std::runtime_error("[UnitreeJoystick] Unitree High Level Control Button " << high_level_ctrl_button_ << " not recognized!!"); + + this->declare_parameter("damping_button", ButtonMap::LT); damping_button_ = this->get_parameter("damping_button").as_int(); - if (unitree_home_button_ > 10 || low_level_ctrl_button_ > 10 || - high_level_ctrl_button_ > 10 || damping_button_ > 10 || - user_pose_button_ > 10 || user_pose_button_ < 0) { - throw std::runtime_error("[UnitreeJoystick] Execution FSM buttons exceed number of buttons (0-10)!"); - } + damping_button_on_layer_ = isOnLayer(damping_button_); + damping_button_on_axis_ = isOnAxis(damping_button_); + damping_button_on_dpad_ = isOnDpad(damping_button_); + if !recognizedButton(damping_button_) throw std::runtime_error("[UnitreeJoystick] Unitree Damping Button " << damping_button_ << " not recognized!!"); + + this->declare_parameter("estop", ButtonMap::RT); + estop_ = this->get_parameter("estop").as_int(); + estop_on_layer_ = isOnLayer(estop_); + estop_on_axis_ = isOnAxis(estop_); + estop_on_dpad_ = isOnDpad(estop_); + if !recognizedButton(estop_) throw std::runtime_error("[UnitreeJoystick] Unitree ESTOP Button " << estop_ << " not recognized!!"); + + this->declare_parameter("vel_cmd_x", ButtonMap::LY); + vel_cmd_x_ = this->get_parameter("vel_cmd_x").as_int(); + vel_cmd_x_on_layer_ = isOnLayer(vel_cmd_x_); + vel_cmd_x_on_axis_ = isOnAxis(vel_cmd_x_); + vel_cmd_x_on_dpad_ = isOnDpad(vel_cmd_x_); + if !recognizedButton(vel_cmd_x_) throw std::runtime_error("[UnitreeJoystick] Unitree x vel axis " << vel_cmd_x_ << " not recognized!!"); + if !vel_cmd_x_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree x vel axis " << vel_cmd_x_ << " is not on a joystick axis!!"); + + this->declare_parameter("vel_cmd_y", ButtonMap::LX); + vel_cmd_y_ = this->get_parameter("vel_cmd_y").as_int(); + vel_cmd_y_on_layer_ = isOnLayer(vel_cmd_y_); + vel_cmd_y_on_axis_ = isOnAxis(vel_cmd_y_); + vel_cmd_y_on_dpad_ = isOnDpad(vel_cmd_y_); + if !recognizedButton(vel_cmd_y_) throw std::runtime_error("[UnitreeJoystick] Unitree y vel axis " << vel_cmd_y_ << " not recognized!!"); + if !vel_cmd_y_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree y vel axis " << vel_cmd_y_ << " is not on a joystick axis!!"); + + this->declare_parameter("vel_cmd_yaw", ButtonMap::RY); + vel_cmd_yaw_ = this->get_parameter("vel_cmd_yaw").as_int(); + vel_cmd_yaw_on_layer_ = isOnLayer(vel_cmd_yaw_); + vel_cmd_yaw_on_axis_ = isOnAxis(vel_cmd_yaw_); + vel_cmd_yaw_on_dpad_ = isOnDpad(vel_cmd_yaw_); + if !recognizedButton(vel_cmd_yaw_) throw std::runtime_error("[UnitreeJoystick] Unitree yaw vel axis " << vel_cmd_yaw_ << " not recognized!!"); + if !vel_cmd_yaw_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree yaw vel axis " << vel_cmd_yaw_ << " is not on a joystick axis!!"); + + this->declare_parameter("layer_button", ButtonMap::LB); + layer_button = this->get_parameter("layer_button"); + layer_buttonon_axis_ = isOnAxis(layer_button); + layer_buttonon_dpad_ = isOnDpad(layer_button); + if !recognizedButton(layer_button) throw std::runtime_error("[UnitreeJoystick] Unitree Layer Button " << layer_button << " not recognized!!"); + + std::initializer_list values = {estop_, damping_button_, user_pose_button_, unitree_home_button_, low_level_ctrl_button_, high_level_ctrl_button_, vel_cmd_x_, vel_cmd_y_, vel_cmd_yaw_, layer_button}; + std::unordered_set s(values.begin(), values.end()); + if !(s.size() == values.size()) throw std::runtime_error("[UnitreeJoystick] Unitree Joystick Configuration contains duplicate buttons " << values); + // Register publishers this->RegisterObkPublisher("pub_exec_fsm_setting", pub_exec_fsm_key_); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick node has been initialized."); } + static const int LAYER_OFFSET = 100; + static const int AXIS_OFFSET = 10; + static const int NEG_OFFSET = 11; + static const enum class ButtonMap : uint8_t { + LT = 2 + AXIS_OFFSET, + RT = 5 + AXIS_OFFSET, + LB = 4, + RB = 5, + X = 3, + Y = 2, + A = 0, + B = 1, + DL = 6 + AXIS_OFFSET + NEG_OFFSET, + DR = 6 + AXIS_OFFSET, + DU = 7 + AXIS_OFFSET, + DD = 7 + AXIS_OFFSET + NEG_OFFSET, + LX = 0 + AXIS_OFFSET, + LY = 1 + AXIS_OFFSET, + RX = 3 + AXIS_OFFSET, + RY = 4 + AXIS_OFFSET, + MENU = 7, + + // Modified by layer button + X1 = X + LAYER_OFFSET, + Y1 = Y + LAYER_OFFSET, + A1 = A + LAYER_OFFSET, + B1 = B + LAYER_OFFSET, + DL1 = DL + LAYER_OFFSET, + DR1 = DR + LAYER_OFFSET, + DU1 = DU + LAYER_OFFSET, + DD1 = DD + LAYER_OFFSET, + }; + + static const std::unordered_map ButtonNames = { + {ButtonMap::LT, "Left Trigger"}, + {ButtonMap::RT, "Right Trigger"}, + {ButtonMap::LB, "Left Button"}, + {ButtonMap::RB, "Right Button"}, + {ButtonMap::X, "X"}, + {ButtonMap::Y, "Y"}, + {ButtonMap::A, "A"}, + {ButtonMap::B, "B"}, + {ButtonMap::DL, "DPad Left"}, + {ButtonMap::DR, "DPad Right"}, + {ButtonMap::DU, "DPad Up"}, + {ButtonMap::DD, "DPad Down"}, + {ButtonMap::LX, "Left Stick X"}, + {ButtonMap::LY, "Left Stick Y"}, + {ButtonMap::RX, "Right Stick X"}, + {ButtonMap::RY, "Right Stick Y"}, + {ButtonMap::MENU, "Menu"}, + {ButtonMap::X1, "Layer 1 X"}, + {ButtonMap::Y1, "Layer 1 Y"}, + {ButtonMap::A1, "Layer 1 A"}, + {ButtonMap::B1, "Layer 1 B"}, + {ButtonMap::DL1, "Layer 1 DPad Left"}, + {ButtonMap::DR1, "Layer 1 DPad Right"}, + {ButtonMap::DU1, "Layer 1 DPad Up"}, + {ButtonMap::DD1, "Layer 1 DPad Down"}, + }; protected: void UpdateXHat(__attribute__((unused)) const sensor_msgs::msg::Joy& msg) override { - // Update velocity commands - constexpr int LEFT_STICK_X = 0; - constexpr int LEFT_STICK_Y = 1; - constexpr int RIGHT_STICK_X = 3; - - unitree_high_level_ctrl_msg high_level_msg; - high_level_msg.header.stamp = this->now(); - high_level_msg.v_x = msg.axes[LEFT_STICK_Y] * v_x_max_; - high_level_msg.v_y = msg.axes[LEFT_STICK_X] * v_y_max_; - high_level_msg.w_z = msg.axes[RIGHT_STICK_X] * w_z_max_; + vel_cmd_msg vel_msg; + vel_msg.header.stamp = this->now(); + vel_msg.v_x = msg.axes[ButtonMap::LY] * v_x_scale_; + vel_msg.v_y = msg.axes[ButtonMap::LX] * v_y_scale_; + vel_msg.w_z = msg.axes[ButtonMap::RX] * w_z_scale_; - this->GetPublisher(this->ctrl_key_)->publish(high_level_msg); + this->GetPublisher(this->ctrl_key_)->publish(vel_msg); // ----- Axes ----- // constexpr int DPAD_VERTICAL = 7; constexpr int DPAD_HORIZONTAL = 6; - constexpr int MENU = 7; // Print control menu static rclcpp::Time last_menu_press = this->now(); - if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[MENU]) { + if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[ButtonMap::MENU]) { RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick Button Layout (XBox Controller):\n" << "Execution Finite State Machine:\n" << - "\tHOME: " << ((unitree_home_button_ < 0) ? "DPAD_LEFT" : BUTTON_NAMES[unitree_home_button_]) << "\n" << - "\tDAMPING: " << ((damping_button_ < 0) ? "DPAD_RIGHT" : BUTTON_NAMES[damping_button_]) << "\n" << - "\tLOW_LEVEL_CTRL: " << ((low_level_ctrl_button_ < 0) ? "DPAD_DOWN" : BUTTON_NAMES[low_level_ctrl_button_]) << "\n" << - "\tHIGH_LEVEL_CTRL: " << ((high_level_ctrl_button_ < 0) ? "DPAD_UP" : BUTTON_NAMES[high_level_ctrl_button_]) << "\n" << + "\tHOME: " << ButtonNames[unitree_home_button_] << "\n" << + "\tDAMPING: " << ButtonNames[damping_button_] << "\n" << + "\tLOW_LEVEL_CTRL: " << ButtonNames[low_level_ctrl_button_] << "\n" << + "\tHIGH_LEVEL_CTRL: " << ButtonNames[high_level_ctrl_button_] << "\n" << "High Level Velocity Commands:\n" << - "\tFront/Back: Left Stick Vertical\n" << - "\tSide/Side: Left Stick Horizontal\n" << - "\tYaw Rate: Right Stick Horizontal" - ); + "\tFront/Back: " << ButtonNames[vel_cmd_x_] << "\n" << + "\tSide/Side: " << ButtonNames[vel_cmd_y_] << "\n" << + "\tYaw Rate: " << ButtonNames[vel_cmd_yaw_] << ""; last_menu_press = this->now(); } // Trigger FSM - static rclcpp::Time last_Dpad_press = this->now(); - if ((this->now() - last_Dpad_press).seconds() > 1) { + static rclcpp::Time last_fsm_msg = this->now(); + if ((this->now() - last_fsm_msg).seconds() > 1) { unitree_fsm_msg fsm_msg; fsm_msg.header.stamp = this->now(); - - if ((msg.axes[DPAD_HORIZONTAL] < -0.5 && damping_button_ < 0) || (damping_button_ >= 0 && msg.buttons[damping_button_])) { - fsm_msg.cmd_exec_fsm_state = 5; // Damping - } else if ((msg.axes[DPAD_HORIZONTAL] > 0.5 && unitree_home_button_ < 0) || (unitree_home_button_ >= 0 && msg.buttons[unitree_home_button_])) { - fsm_msg.cmd_exec_fsm_state = 1; // Home - } else if ((msg.axes[DPAD_VERTICAL] > 0.5 && high_level_ctrl_button_ < 0) || (high_level_ctrl_button_ >= 0 && msg.buttons[high_level_ctrl_button_])) { - fsm_msg.cmd_exec_fsm_state = 4; // High Level Contrl - } else if ((msg.axes[DPAD_VERTICAL] < -0.5 && low_level_ctrl_button_ < 0) || (low_level_ctrl_button_ >= 0 && msg.buttons[low_level_ctrl_button_])) { - fsm_msg.cmd_exec_fsm_state = 3; // Low Level Control - } else if (msg.buttons[user_pose_button_]) { - fsm_msg.cmd_exec_fsm_state = 2; // User Pose - }else { - return; + + if getEstop(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // ESTOP + } else if getDamping(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // Damping + } else if getUserPose(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // User Pose + } else if getUnitreeHome(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // Unitree Stand + } else if getUserCtrl(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // User Control + } else if getUnitreeCtrl(msg) { + fsm_msg.cmd_exec_fsm_state = 5; // Unitree Control + } else { + continue; } last_Dpad_press = this->now(); this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); @@ -106,10 +223,70 @@ namespace obelisk { } } - unitree_high_level_ctrl_msg ComputeControl() override { + bool recognizeButton(ButtonMap btn) { + return ButtonNames.count(btn) > 0 + } + + bool getEstopButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, estop_, estop_on_layer_, estop_on_axis_, estop_on_dpad_); + } + + bool getDampingButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, damping_button_, damping_button_on_layer_, damping_button_on_axis_, damping_button_on_dpad_); + } + + bool getUserPoseButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, user_pose_button_, user_pose_button_on_layer_, user_pose_button_on_axis_, user_pose_button_on_dpad_); + } + + bool getUnitreeHomeButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, unitree_home_button_, unitree_home_button_on_layer_, unitree_home_button_on_axis_, unitree_home_button_on_dpad_); + } + + bool getUserCtrlButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, low_level_ctrl_button_, low_level_ctrl_button_on_layer_, low_level_ctrl_button_on_axis_, low_level_ctrl_on_dpad_); + } + bool getUnitreeCtrlButton(const sensor_msgs::msg::Joy& msg) { + return getButton(msg, high_level_ctrl_button_, high_level_ctrl_button_on_layer_, high_level_ctrl_button_on_axis_, high_level_ctrl_button_on_dpad_); + } + + bool getButton(const sensor_msgs::msg::Joy& msg, ButtonMap btn, bool on_layer, bool on_axis, bool on_dpad){ + if on_layer { + if !get_button(msg, layer_button, false, layer_on_axis, layer_on_dpad) { + return false; + } + btn -= LAYER_OFFSET; + } + if on_axis { + return msg.axis[btn - AXIS_OFFSET] > axis_threshold_; + } + if on_dpad { + int sgn = 1; + if (btn == ButtonMap::DL || btn == ButtonMap::DD || btn == ButtonMap::DL1 || btn == ButtonMap::DD1) { + btn -= NEG_OFFSET; + sgn = -1; + } + return msg.axis[btn - AXIS_OFFSET] * sgn > 0.5 + } + return msg.buttons[btn]; + } + + bool isOnLayer(ButtonMap btn) { + return btn == ButtonMap::X1 || btn == ButtonMap::Y1 || btn == ButtonMap::A1 || btn == ButtonMap::B1 || btn == ButtonMap::DL1 || btn == ButtonMap::DR1 || btn == ButtonMap::DU1 || btn == ButtonMap::DD1; + } + + bool isOnAxis(ButtonMap btn) { + return btn == ButtonMap::LT || btn == ButtonMap::RT || btn == ButtonMap::LX || btn == ButtonMap::LY || btn == ButtonMap::RX || btn == ButtonMap::RY; + } + + bool isOnDpad(ButtonMap btn) { + return btn == ButtonMap::DL || btn == ButtonMap::DR || btn == ButtonMap::DU || btn == ButtonMap::DD || btn == ButtonMap::DL1 || btn == ButtonMap::DR1 || btn == ButtonMap::DU1 || btn == ButtonMap::DD1; + } + + vel_cmd_msg ComputeControl() override { // This callback is not used. We send the high level control in the callback, // for safety if the remote is disconnected - unitree_high_level_ctrl_msg msg; + vel_cmd_msg msg; return msg; }; @@ -117,9 +294,9 @@ namespace obelisk { const std::string pub_exec_fsm_key_ = "pub_exec_fsm_key"; // Hold velocity bounds - float v_x_max_; - float v_y_max_; - float w_z_max_; + float v_x_scale_; + float v_y_scale_; + float w_z_scale_; // Hold button locations for execution fsm int damping_button_; @@ -127,11 +304,41 @@ namespace obelisk { int user_pose_button_; int low_level_ctrl_button_; int high_level_ctrl_button_; - private: - // Button names - const std::array BUTTON_NAMES = { - "A", "B", "X", "Y", "LB", "RB", "VIEW", "MENU", "LSTICK", "RSTICK", "SHARE" - }; + int estop_; + int layer_button; + bool damping_button_on_layer_; + bool unitree_home_button_on_layer_; + bool user_pose_button_on_layer_; + bool low_level_ctrl_button_on_layer_; + bool high_level_ctrl_button_on_layer_; + bool estop_on_layer_; + bool damping_button_on_axis_; + bool unitree_home_button_on_axis_; + bool user_pose_button_on_axis_; + bool low_level_ctrl_button_on_axis_; + bool high_level_ctrl_button_on_axis_; + bool estop_on_axis_; + bool damping_button_on_dpad_; + bool unitree_home_button_on_dpad_; + bool user_pose_button_on_dpad_; + bool low_level_ctrl_button_on_dpad_; + bool high_level_ctrl_button_on_dpad_; + bool layer_button_on_dpad_; + bool layer_button_on_axis; + bool estop_on_dpad_; + int vel_cmd_x_; + int vel_cmd_y_; + int vel_cmd_yaw_; + bool vel_cmd_x_on_layer_; + bool vel_cmd_y_on_layer_; + bool vel_cmd_yaw_on_layer_; + bool vel_cmd_x_on_axis_; + bool vel_cmd_y_on_axis_; + bool vel_cmd_yaw_on_axis_; + bool vel_cmd_x_on_dpad_; + bool vel_cmd_y_on_dpad_; + bool vel_cmd_yaw_on_dpad_; + private: }; } \ No newline at end of file From bb124b009aa9dafcdde4d23482b535f39f4892e4 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Wed, 5 Nov 2025 18:24:15 -0800 Subject: [PATCH 03/27] joystick docs update --- docs/source/images/UnitreeFSM.png | Bin 0 -> 39362 bytes docs/source/index.rst | 1 + docs/source/unitree_interfaces.md | 85 ++++++++++++++++++++++++++++++ 3 files changed, 86 insertions(+) create mode 100644 docs/source/images/UnitreeFSM.png create mode 100644 docs/source/unitree_interfaces.md diff --git a/docs/source/images/UnitreeFSM.png b/docs/source/images/UnitreeFSM.png new file mode 100644 index 0000000000000000000000000000000000000000..dd03cc7ec0881eef9c049dad0c406e1a63b365bc GIT binary patch literal 39362 zcmeEu2Ut`|);3X4lqgw}2uRLBBu9}9N>GAGme8b@+<<_9fGCJ4QAvUd2uRLZ$zmf> zqLNXvgckq04LHv1?#z7q@6ODA-#(A?OmlD5sj5?_PQ54HFf9$`V@GI?U|?VzQ#q%g zgMons$H2h6gntM;@%Z?R0|P_xx|^bbn}dgyt-U1%i=h18FBSnl8#AmFW~Qrk$17lTAJ@~&OSvm7Z;fO&zo7n?DvDxc5!c(4rT|9_aC_0m|4Kw_xhtB{kScAj67hc z3k(M6{jh64zCLK=;302m4{`f|kNe{Qx%r>>{mu=S?-Kt1L(WstL(JXAf}79WgZrw1 zryR`1Oq*M5k1?)po(BSJX#se>|0m4F%?4%-b2PJ8{OPHj3k>dPVF_&+5M4m#pp6;~ z<^&!>NA8-Xo15pp;F!VPVBnFBn}hxSTT2gHH-jI38$zUbgh3~GcpLn>>R#zC@#e84P{w{0w+qs#!SOZ;y)FbF;nvBNd?D_Ros-_jb@h{XOyo+U$$t zug?Y2wZD+szq?NUec}5n$cpGcCoFs2{#0oZ+!M$@P+ER{_xB|6KptgXT+AR*Jdipk zn60CmD>UWbPvwA?gNGK>8&KQ7c?3lEB?vm3fTpmJ@E$XMTbh5IP@1ies z<`?t-chmpp_#-F}__?={zh~3X%>h<8&@aEFvih2XECRs{YzwSw;T zr)m|UKU}LoC+4>?&A`DyN#`zrLL11`nGom znE2;tWUu$%K?nfeyIJn*u7CDU-|sH^3(au=NZo&eulx{4p}XDY2k3gBF%R_dUjPLh zz^DHr#=im6`$+IN5#t|^0sfrRy3davPOE~fx|WKD@?ZbJ3Lc!s1FZ6&dt&#c?T1}Z z)V-jo_1E76;RA#IKYtJS_gyYO+);m^o&A$-ZFpcn{|-BQ&mf0j4b%&|NP!#T-(Y9& zW0&95n)@vL11+aNM{DvQI1&GQ!~V?;`+H?&-=_HAFDrkl|L*5{1+5|e$9mfnEnrv1Sl{mc1Gf9Zk|JP5b^2RcpnG3F2K{zIYHL0H4^z>xn7 z!Sww*_&3e}KVAbvaO95Wv0cd^ z{W2o+9}LMLnf(R3|3FA~Uwkl!mU|5uN}n89cvNhyU@;l)uce6#C_{{O8=kdp6JCGkAVM|ND6VqGSIz2HU}S|1!Tj0RT{txtJ|AiFr?}bG_Bo6$0_3yqV^_v#g|6%E)nCH;TW;q(vnyNJ-wvlRYG`u}12S72WO|2eSh=ZR(qrqOSrF0^_$NMQYLNdMFg z9ijODceDK7L%+|$KhPHXliV!(4EeKU<)4X3{33$&6JGz*q~(uyv;1Ws`Y$2wzdX_G zAZiAkec*)@Di_J>!dn|`X(xk=oV)PTcM-Y5uF^HrW!_$& ziau9ul;;+0xcf zvrQr_t~sYwnKKxe#`uqT3#YMDdAuXWF~P*U@W-acshvu(u_(YBZkh9Ea8h|#{Ke?N zJ74hbaTJ*X4o-L6Kp%#J@di}`HW)3Lq_(3$aLfr0hl^Oo^pgKQ7kmZd^BB(J;N3QH zZHGJqTPQv>P9A1x`2zn)LTbd;>=R>X#VoABjWD?_o^1fp=?6;GL8P zF$th%Wr|GO+ik1uazcV5v(H7>@MGm?38wuLNLh_h6;plV_aM!3uo~cBDH5yxSI&RRGi;`W}-( zT1N3pU|=%d;m2yYc`S`y(yO~R0NZwZZT3Q`r`zX|B*a#mY0pdBw>C4~kExY0SHZZ* zAczm^nA&lY%}Zc+WSa4X90u=Ea2d#$BDPq*e*NtCq}XAor#_g#_DRz9REOe$n}bDm z%58DHU1CQb;9-GX(wO9^u2WvmUbh9LaoNozlPg(OKms>3+`{#S916vCUBuHM(+x5eky#|I126#Z$J zh^a&!Przq;f@wtT&pp%2Ni{6AdXo`nGWYuIFj%z~85ZDlAKnigPABIya3!&gjUNKL zgrRNybB@dT_q|(R9ExMrOpYjWE6 z-yavyr(n!Dfu$WsDw9ZvBd6%!q}i1$ne95=i7c>Gz1EYil9jE%@Z}acr_JkV7OTjy z$tdaDi-sqyPWVz2meK5xrt&0i%cjfJQe#zlf5{|aR_aD(>qcrBytE-%#yt|Efn(rQ zV*9qa5~t^xJ}-@g2cP;w0Y&QY7A{7K zk8`s&=rEbh@06gg;c2Tp1)nE)0REmLiH z-gHNTG79^P#|IkCfFNsif8Z4@i5ntY4W(MB~vGoyI z>FGM5j4d`H=g*MghgxnCvTA=&O4=qXGSlXva(_-WwA{n)$V1G4BRJrNEYJ_BWv8%9N05H>&_F`hKe162AYyc zw*^cP84!0y;^nMl9${fFe4uyd_aEik!iAZwSkV^Y)DXjq7p|Nj#;3vz3sL^aV zg3QDODXWf|(lJEcyoQ-B*P8SCkm>gc+SzUtvCf_ym7MAjAL}0F9h_ zui*^wQlNihQS`IZzPjEW>yO=M)bQW&^p=%$TTHD_;#2=gsrWW1=i#kc{jwJOxtWD{z&UBX1pE}?K(!a5T8SjNv z@=1@j6IZqIAj)Angwry$L|CMhl+q9D2$(St4bQQcC|F^|+Nw(g+SYA^&6olHBzH9>%l#iTbELKndV5fgX&}$)u4%)og}wHj!{+yhsg|UtF8f|8Ws*- z@W-&ianZsi6^%Mk8}E5*jN3I-k?tIz%y#}vA~vQp#7mULA>Ig#&0~dxwU-}M4k&Tg z@hT*7pQXaX!BT>F$!Am-Oo#|fJ1RJ1rgQ)FwJ;!2stP2Bumku=mf+4?9WaFa+f4N| zWd`4uSXsvI?_*7T*Dnq_x6pNrmS0~yA9(0ks`2%KV5!|r^LR;bk@r5U9cy2|jJ{3w z{-nZNgA(xl^4|TTze%|pJDtZ^Kx6SPYFb*~$K;tF_cm&^Yc~5yaw?Pj&F<~>`Dcmx z_EFM)-@bn7Ft{BSwOBHm>{z+1B>v%!yv=-0NhU4; zsjaELu-g@2V#IY<|eOm*xF+y>GTdbTobEQ$ZX|UZ1PJ*t9?3pMpWR zUxH(K*zGe@RN`a)*o<>4$aAmA@jo2HG!>1~NzGM`qF3r(E?KBwPGk5Q+A00bhq<_w z1a2P!lamP4-%9QB{DoRu78D2|F(Nqz`6n;7OU-pt z)=0U8y~gX(TDD)>T%MfGt@t!}Zg>mY)Jf(0l^@2!d8_%BtC#8tXeEo)IOaUUaKi`L zQhcIW)gEXC52oy{J!wq+63Tr0v`Sfr8eO;oVy!jG&$E~~%2c)f7ABZ?&@KPG1Z8&Z zmdzrYH}yW=(kri+vprX*rV@nnvL_$jP|wNU0l)`VT}Am0>ATt?N-lW#vVA`8dzUtD z+K!%O{~J~H9wrG5QO+Jy0e#42aR$WE%kRBX0LKO;qCY0~G%loB`7F;f$35@?xa)oB zndIfGceS}b`K(wm&lL||2M0}r_PzV4{F^< zJVUlO7FA(Y>+h6?SB^zm&v`WSsJBbqYDVuuM+=ZTD-*1-KvS_Xc#?!_hpB>x!zFOx zR;Js7LGbDe2d$GmS&jX^uDf;1;yvUwR+l$!7bxBSx@4}0kj?hUyxn8~ zu+r>ZW%|dkny>GIECh91#0VLr5=BORTm^RswWl+lkKy1qhGC3eC}NOuq?agcin`z_ zd1?GgezV<($GZsLPSm+u*1SjX{1T>V4@K?decR4!)lRX%0!QeFe_^LkLJx0)JhMQRnMs?(hZHnpP}j%oTHW_ z=>420c+KrgoKArAuavl4aH}3L-h)fJhmQYh&{pabMPym-Yn}BQC8oyAgx+NeGCOKv zd7GQsM@$>>2v69IaJ)AQp=;m3;kxBvhl3D|9M!AXoCxpG!$XPPGmRdDO@+{V*-Shv zm&;!+@j^tL^<4R&keaI!!)moiOdv*)UMZ8vKoCVE+*&EY?>g0vVV_;dpL0S@M2Uy-#wZzoGI}-8_6#X|<&0 zt3#TwQ!M`^>{BJ~`Q`hp5;rN0N*v$E1ML!f4!L}V;=1Ut6E>aCQk|7jK#9uOKpD`y zx%GPS52yp56Q!W6l2p|;Jk{x70}b#t(<`hRi8dQDT$RV>6G|G0q=$WGC*p!UHkP9N zP-(MOmTmW``?s8Db4o_8jciX9Jsl>ftRnJT9uKredWckcq=-}4SL$1UY4{L+UiJXj zPrPzzaLF}-6RRtStM!%y@TyJtcmT>3xRLvUjV`JvDSNWtqr*Oq$5?Hl@}kaox$e^I zK6(Udanh+HFH$m8oT0s5c@4i`DB%?T;ABiA>!HyxeYWu664US|Bc@ABnK7dJ+GSIH z&A{~G+_=Klhi)qKL(}+;?6+ZB!`fw^RB645;m&=>3s!^t)eAk3WLy$GXeX*h0l*^w zLyPgIP%AVv&BDY?u!_Oa^7e`DZycjV-F$s=51bmW+L;rjcHXCpzy@|q7G?&1ztKYM z&E)wPwU>w2D$uh*!!Ze_D{t+S3UHyeRja<-4M8*GQ3tO1_ z(dxQ94T>%6dV_Xgz*R3O=h=SZJR(reBe z?&X}6G_~DEQ&VKur&Hy%&cSG0;z-w2{Vu^kw-(xz;KKLC`@7%2-Kq9V$L%#YUBQ)6) zb|rDAC?h)PG5gjh#3w47^*wfq4BE+;dT)o%i@!>c>_ZLq2WGWhj%6BEBLUPmD z25pdn=W?=hdFRfL0C_ex5WZm(dlI58*XPKq!b6oHiIqFw1aAz@^HM09oG2$;vFZ}8 z5!nEENYC<>C>h{6cbNI&M)Zdp`2);je;vt`A7HGY2A-aV&2Mk91ug^G$61B+Stb~?~A zZ~F7idzI$Pla1Lp$QibKgk!uGEZ@Y5z>jD@d2)zYr53$$YeqwMgD=@T5=5gsMxSr3 zPO}}xH7958uJ-kgm+}>d&vcumF@*OWFNW5Vu)mfUaToX2a+^lxcmR4WI&Kb8zF$7Q zPXHbsdE<)f>ZC`SIh3vdUI3PyiWjiFHE}wuAj>-LuxfKiwcG1-!pny18R|)L$k{AR z1rjkVk&gSP6#F)gRI{l)kO`;cWkm@2Guz{AYJ1JU%gHvaLClf8ewu8|!b1nvLLRen zyZsNbLiB`!`%?EmVNSd~V?4M9=bLUSocC>lWp`MHwBdbyYw!$IW<%bE6~5$AC&LNj%o7 zBVG}pmHO*!D)=iB_e+#*-ew+yd_dr)u*50bP|l5IYAsKG+w+)3YR&Qz>(}JZ73dA? zgdMBBoQ|f5G#+}^DazaP=}lS)$r0|$#iy!e#=FBXKJ%7Zye5ULJ@o_;^>tN}UZ$C} zIJ+5j8k( z=@YLdD~3 zwu>xvpAf?lX97yjp=CU~qByS6Ji*YwjHii zyalc#7M1r!Ri~=u>DugOJ0(`to|@K3<(Sqa14~kU)MxdWda?wy-F%QruSwV7q^k;p zLXxV`*TI5p-B9tG#2>Z@@U<~N*sxsK?%e~1l4;XRW2T+J?mgpB_VK<4ncjm7dvjQm z{gpYKIBVa}C%*Xdtxc^z<`mv1f=xK^dMZWIC`L(bpFTz>F4Txj4K>``D-xN1Ov#*z z0OgjSl|%$sWI^^T+tFY!_9R?!(Z?pLizVS#HiD`dOWK(0+O#~_-+`bk*RV{}%aB%> zjwAC3KMt;J)#!3NZxx>wK8nrRtJ-H70Iy!N{tB;d=GSUch7WFa)2FD4FwGgI&i37h zPWIMEoRO1?kgiPsG-yIP${&G?C^tZMhHPva6&%){CRMK~wg(EWlb3CYf*pN?;e8v? zo43ulmfkj{UYB~=G99h{D?)M9JRpUKqG^HrU_)1tv9BEsHad-mfLgejud}JL z)!S)!wDihNRqxm)h7@%MS!9zb-m7kGv;-gBli=sa5juz>R|;$%V))=QuH8-hZTwW8 zU2&P4^k{NsrI1!TYM!wav+^vdkjg0_>#d39a*Op_W#F*x!xUKahZNf321ucqQ*j2T z+%sKFZ1N{iNZQ~CmzGM9x{(!LLwM&g9$Fz&p%rq20Xr%kb#0pKkP+kDOMOGs^1vF2 zf-ws2e^e5UvFgzppQ`!%INL}4v)24$SlUmjS|_5qGNTyhCi1W?W+2F}N_une{OcW& zX0I7M6OKt4D!zD;!p}#<7e|}YkB(8E2z{IJ8U2DMGDzFJg3ZaoS?194&3O!JFUTB= z4jSJ({jv3$t$Eiwuh!zGYW$aVe{t5$`G|tpo7cvIO43>P%=_Gzb%50w9Tb{RPhyaN z6_?wvAP53PQPR6&+@{sTXlF_c6Icu<*-1DvMd6Hxz{43lEmVesW+9^-PYJd-M$rLj68%huybb$VnM z->olT<1?fZ`pyQbxJ&tb$$-vW*(BwCmk8*P1hf=Y-T2%bN&9wbydfK4G$pF`6lrM{ z1FB~j0UL`$w6fK1vpO^a7Wg?vWjeAKghTFJl-yC1P}L5pUVCjJQth*brfAu~`+Vvs zbO5lz?7VSD$yn7MHHA~YeRY>PCMYof2nm&t4RH~04FJ#j6Tb4NOqLhBq{zB-z;b;B z9_Hk6?eswBsNf^zJJ%cNiPaZL0{nRW?_%QhsH=rp)}t>iSmwIpsvowGnCk7ZYhaat z=nVSc)Q%0(^HwK|!sr2Zpo=n!A%5(`CB5=+517lYw~^(}8{W9jr9ii>bK~`buT^gx zbVB1KYnJX{Lwe!x(cDGje-Ol?DheyhwGLA#B!z(B zn9FpCsYloeG*1KUya_0I(lwIA&4!{+ox%~gUvlw*pfzEU!;qdsBUs7FH+w5lk(Mf) zQH3>~jHmvxj&!T4xt|CP8t{(WPz@0$FG1{yRH z{>(JY-`KQY0r4=|!{!{otosBx4jvy0@JI6~EFpaZ*wad1hFt+of3<78D$l7nqh!=k zE4241O?~(QR=dE}O*Kc9s`1?C27FGj>c{LHhct;c z;Mb!MXhrE!xIWX*%e+){?cqc5kuHt7(8s%sbLmgnjB566-qQ6OugD>!6ww(Wb2C&%N7K{3Hz{%s-oIWmv zT9V&hj`HJ*S3k4PMdKvg$u#$pKUJE(4-Nw^1`R)B#(E6&J!bo&%H{TfM?vGeM}Z+V z_k==&*b^<@*sZBoH9Mcq4uU9F!?~0Gr=_=Nm^Ky9TCj0%=`qxM42GA688bWw&aGvYT*s(Z(CinzLx^x)i_Nt!cCc27=%1{U9`U=eG2&$2JJqJiAjX9RmV9 zQL(5>C!P1@@Y$*_BWb!CXM#rnpN?Eck%gkZi8u^Cm^V?c_DFN2UtK8dvN9pw7xXhZ zPIu53zv_u2uo{Z+vKyMSn+5H3#m{b)W6{Sp(dFe2gD7FsBe|~^iG1}+`$NS?ux69o zNBAdl7>l2h!IkMfEfkkP5(Ulr!oXYnhWh;Z+=`wy-Ws)zc;RQ8AcD83)Fu6`a?6F^ zVNi&95Q;Na?S925g z;TRj)g^C=V&ceM6z=hc6sn>UmR=Wi&(%Rj#I8u?EAyPZ+ znok`I>YJ31>oacxl_IEQYj)T3X1nEyGP3!FWYYCq?wPl(KU8l`(PA^_RSaDX=Z@e`&X_dUwVT_#J39ocFsVpnt^svw94hrU{~a%K!?!-)usJ%pn6`7<;jZ(E<>?R^|!#U z9@2UCvjlL*`*jM!eca~XMLL$xvU-gNQ`!s^SY8}KsB_F6*-iGGqAvejy)*NgEyayz zt{<;vu@yG0@qn-H!+Nj2sVwrVKgI|mcFt-u@<_mQp3YpE^JJqi#47FTA=?cso zw0^WBP{2ElcEsuma5=bv5Vd~9c*z{NTCGR!aoR`KisC0Ljs@1dS>1k6uZORCpNl_A z>%2ph)2FiO9&jUb7GFFq_5n8MGs&^;X{i<^Hi7SoJ-Nuh!QhiNmO~HxYUaD`XGT0G zBdrcy1+i|Cl;vW45a*8GN`&31RK$_4pAbGD8zud9R`VtBqsFV-feS!$HS-G7biLS! zSE|^}Q^QZjMZ;2DTCPRq4OjLg$C2|?R%rtdkj>`O_|!6Mp7Z1Y-)<8LBKwg6(wQC= zjrpYk9I;siWWMBxNRV~U`Hsfe?Mm@PkYREyms^7DRn!#%W%kP+sQBV*&Qip$F!we; zj#Ubr3xc>B)QYM&YN>%!VJK6nSq#nvGSES%@a%Qf?dxkl66wp$l+Ww zCO*rGt1)&&5=;IMNyO%|(js0|ekz}DH)o}Qa}iE=CaHpeY>rdiQ0UUAUapZL)11el zz!D!P&-C#=%M%F&8In>AR2Qb1IgK{IqskWe6q{OZV;#u=~2Lix2|fp z@Ls$yA{bQ@x;$y!7Xm4Fv&>H6@E38a5GYZ-QZU@psD!LqZl+xW$CLz?QWxdE>BKVE zulI40Ko=yhg(r7KPxB5KmXlkwn+6m5&#P zGs3#`kjtXhyP1AXE1%;*hsIiEDa^ z{xF1R;7=<_*t)2e>!b;KUzo`k5=MLpp9uaMJbFl-MwwK*rP%P*QaG7J;lYK(P&*~; z7OP124#|>J`=Z6VH_j|AI%>vvloDdu$FyRXBo$Rqc*G|!uh7HBT^roKPLFn5>Me+T6 z_fd5SK-uM%WuB*Awm-$Y_Z380OGw~AD@-=Rz8br{WLQVWx#vk%r-Irv4sb*xy=l zhH@*VlPz*>eJZMX`$2$3wSlVh7#3!N(z%3CA_n26S9b0mu%{r9^I+Y$a!CAhnx2Q* zNe3TPDu(p=j|0NHk>?Pnyg{Dj!<0i$HIU9nE8!T@hpsctaloQ#QzQLYXTUv}gLQb+ zz#}DR?_ejxj8FyG;V&IZ-oKMDnQAFU>EJK`ff!wq+8o0!War4^TyDQa-JcYEeP(b{HCi<~-|&lNw1Ls>kGzJcO00k)*csA>6URogp_^ z>${o7-dunAdehZhr`v8mw#!Xh!px7L+u!n{DgVYtO}}m3RV7i_d)=+@UXVW89!G9+ zU0cjvfiCJ}#u2WgTLg>>bPC3LU-2vMGLB8&oM=#1ce^6}{M@agcA@n7ybJwEWRSi>``HVf;*W3z59u?^|sE@o>Wy(+@My~T@Gx_|2M_K)9m$W9x)VRfurwyW( zPji<%J4u$D$JfT0k3|Ch#n5&JXAu+~l3P&S?YKDd^a)6a@Y}xq#TE7bGeTcB1mvl} z8X9-bu7saM4ZR_T6T=^G2u^GS`9~2vA!4u{>!CM|A9mtVFwPPtzG`}Xwx>Rg=Z0}%9Q1PhZ}v~FBHjLAT$!64OBn;}^(R5orD zTCQcsh^+RR2xDviOlu0VH`ZpKq{Qc_Z+<+C7>X19paIfq2+{#Q9zty&f%u#CHKJ6K zX>787wn3Su^kqvNt7syDHs!TrL4$FqR2%~FbwoAb)JBUBa>TkUOQSvG}ki4d` zs>J=-xrH`CKN;5|0riDN^*)Yb5&D8sjd^q)r|y2BzdG|h^OBV_fc0VftOpEuhp{bi zVQOJwKURF!k&ojFjy(;NgVzu3gu}}-)f3`PH^*>4>EO-fd3^{7zo59iJiiwcN0bIr z%Az}Y{Lra4T%5ucCqG|HvL*vZsh)$_C+;cKP$HC&#HbVDKV^!gee^Q%5C|-3(F-pPT$x+(8?27HVXS2@>J5zxsdmAro*G@oYp!+0G zr#o(2%$fS`p5<6(L0o(R#G3GJvA_H@%lB(TdAQCkb_>@>eck!9wETMIiHsW0_k^48 zg_2k0xD(HzdrhwX)T>GAcTUdWE%miyk9g%RJ9!-U2o_ErO2bluE``kL2IY9FqMt`qr9i{E#Rn7Mr;RPpvZhZ$0RPfp6a#&F+R)x|A=51eZF zw({U$t49c0wLj8MQ+fmPY~DVoD?360(vO*;ti^Kpd?ufGXnFiOS<|Q71IPd<2>gXQ zv>b`7^S_RkM?y#*^ZmZ3OElBe47XKFTw} zik9eG;EwO+Ia=nF(Nbq~8Yv$!kd=4i5D|TZaz2mDx_uzSVgwccjr%;t=jCr6eH_cF z?{%B8tU_y>+0)9gQ^vHsPP^w;RP{^X02hDF;Sb|Qla%oRB+WmrZrvp!2!vcP&kYN0 z-lQt_e`;i@z3s5Pf&YW&GlH>i_f?Sq&g1nE^X=yyk9l(*>+pBGn+`&k)vX>GVnejZ ze%)ERE7Cl%>UsBi6uo4G7O>A)_VV^U{OW*S>s}Zrv>kkD`_%MS7GWLnj{H;7?l_%S zY+s;AG8BtA6D7vXhu;44VEZHaHK$dVnLt#aq*jqqUdQL$R&Ow3>rMX{*m$XGF+pN< zQ;z>O{Vxap%YpA4s6&{ap%K3Jfs347Dv^cwArre)I;vSN}QkHK9b zQ7`$YW__0K-gfBQEGv%VIh)Ac6moCWfU2Ypq-;?X-^Ej)y9Vk!62ckA28FP4mt7r* zhBM@4ZV|E9%y##<7DE)hHswxHqc3A)^y!yKB9iLKU!;-l+vu?jDa_csvqDQSDY6Ly zlJz1hwQ6+NKwZ=AYqe8TcN=4&ke>NMGyXYn%0=(fy!c^UkmR9#tbbS{y3_?^YQQ3b zv%)hDp#zrt6ZdQOEk3nRBS-L?8&i7 zIRLuvj1k~M>^1GJj+`&R?~FVLOaS`Xwi zopVR2e&(asz+U#)d%(hv{f%(QyVHgfZC%aiYErqiF(f1o54x z*z7e3Es!x)OD%;cubaQfWEvQEkk6eHtBJRQW>M@O{T0t9`I z?6HYBh)=8!x)tE+cjhIGhFp_)7q(<&o)V^`wq?mvHL$EVk1NzF%kL~P;hQ<-LZUby z&)DbrLh?j8@#*z897Bcq@?3lKdnP_N?PR^Ta(yn((qkky!`6x24SB3{+J0-7=mREM7y!zn`^g#{S-#TU?NK}{1Cl4 zZ^>2ZbDhJQl8@JTu5xPg0=dX)c%qNvV|32vN3qN`SaFI}<=RN!>_{*T`$>8sQ4i7Y z#&Tyxv(ByQ4`tmH$m23YrYRHAh&eH;r${9ofyxVhR@T{B7`-wtarb0k*7S3d4Lj4P z8mm)%yw7^Q1GC=NVqJZ89FsxxG=r1?F1yntNPEkGs*&PM5h{M)c1E*72IWhqak)X@ zhO`0p?$*_S%l1T3>+Lu*kD0Zv}L7 zqNqE#^Vfz;osk);l-Kq$-EKi~a;S)EYU|uq2M0gWRqs{nU@Uq(3u-}l^h`quC63SV zBe3gOdSe>5tZ@sHGlr1%NxU_NZ-Tu#j~E(Oc+L}L`>+_^%!*N9sICIeg+m`3LMUIH zbgH-WfT|{d&%e`++nE1?4x;eU(e*^!r{ebKp$VSPKc>wVEP?Wp-3RP4RT14zz)gY94o=W;?#To2%yRb zMe-ox@4-4G5~Oh>iuN86L$0SxN`;cpS?}r{iAJwZLf@MEZ{iTk8j~FWvNo3A&6)>b zJ`!N&OpT;#LG`JM;i1?q2ho21@CNVoImJLcqD*$(%$cKlr#a9fVN@R?X%^^a!62u_49I5k!kaYlQ@GVFc{9(4_TgyZ(avapm{}r z^Jbby>t4}#T9RE9L3A7k4dFLUg*sADm;r-4>9%d5OznAWq~AjdqJ} zweR-Y<6Ps4!Nfp*Oh!J}DYbYZTT=R_-P%|*!))6KeUm9EzvK$1Kbu#NnymsG>cj5G!)ONeTmXpyN-0ld zwWiAq*UL7%-n~{@$?A@K%}+2M@qD|7^~)tu5c{|})saA^Nj(utfQh)byWGO?_5r{7 z#Ca|=5h+J4DW6kn6|GAk=$F_%ZA*;wcBJsltx8@mxXUH5W+_l9N0S6nmOq%zgDAiN z-(He(Zih$2Vf=uAtH&EKFvvbb-vzi$cQ(F5zXCo_p@sY;zYcMU`IrDOM;X^VX>it) z6BJWGerJ{asqmw|50B>EQL+a1=T_gPYM-r}=v)9&+Lu_g4SAQl9twsaJR0c@J~%e) zli>gf1E+qK%-~YPn=EbSgnTM8Myyh?NyWoFmyxpm9SMvUeW>!{IL26`W4N8pZ=oVh zsH7mFj7^sLO{L0teQzHXm%9E)5>O{$=RNk^3&-DeIF@Oz8LoC5nYJiM;DoB|M7A;u z8=F{G-s!|9+aZjHbaC&uT3h|IUO92#m<>GX;3}pg^KGA@nX&=--9G&a$dDZgwZEy-g$tU3f{F0 z4HF+lmN%e)ySkrzg<1Q8T=vwU!#H=vRepU8TsX12r@v_uDwP+$GQr6Kjj`I4dxs$` zmGr!DpJP6QUhXXAfH>1==#V8i;tlNYv`N31hL#&Vo0axdIeziOokI`l=NQioJ`X%p z^G++927lzjn{f%M!w9Ci#J5>(j})|L*N?&RY*Xz!zOSHjR zLm=aN7Us7@yI4M-d!fY97+tG*5C>)STgY$GsX=_5`4FR-**eJpN@K-Aq^zS9)Q~1Z z2@JF%_I%%9qt#zo{TSch*4fe~Rb)vKK6zS6MxM^dsSbClkGr(xTJKX!FQ}|l$ZbYz zdny6OA?FB^n5#hSTF7_XO?*1hm1B1V+~C{2c?iYn6!0C6?Y^4a=m}7@$TkSd3vEFz zWSZoc;TRLdmgP2Zs<%^Zw7{4MsRe?dd;l_R22hidGqCXyY3wW%MwLlbZcQb)f#6WQnD7aj&0c?b8~&i$p+F+3b1e*h z629QrTpX)Ht+dVNx6liMOiWW${n`k~w9xdLj1*P-yfo^!Gc+3Sx;afyHeE)!D2;O325de0Ie`>z z3Ra_h6o#5AHyENpK%#RhUuCX<=2I};S-V{KeEO2h^4HKtd z1wz#G;dM-;2jIZ_h;tJI{JV138%dOjyVA)O=(>u1q2wPZRJ^)*h2)iJD6M1Z<4YiZ zX~%`~`tr#EG$~!yevmzxK|6}r$$6+aHjm@C0}?|_TI;UHSbyb^KH2%8vvEpzDM4)J zL?W@A>HXBltlBNpD``Ev)tmM0AF4pru~l=*x*82oZDRYO=U4QWKU5=wV9evID*t4i|1%K;@2JOowA3*9w z0)f4yGn=z>J~-{q-T zxQa8bNNKD<1P)g$h+&g7ODAmf)$I1RpIEw7F(C}+n%i9FBv35w_;w`qHjG14a{V4j zLU5oty6Du;lY10=vtTZ|wZPtXtmJtuvow29HJ4&t<~t<()9^q$0tQlHrbb%73_(Gl zHLg+jt-B86rzzmWF_l8MtosR6(>Yp}*WPL-C>Kb$c1l)coA8iVtWA^pQWs^J%Wd80 z8ZF)$!ca(Z3v2MZk8ksBd`Z0nxqS7SiCBjqT%uwGxMOdHi{*tt_4V!L2{*FN&Oa}V zz?T4}@-tuug-TocqS9BPEp5M`H3l}B-hisMPDr0PpEbvFG{6Y_dum7<-6PS9i(BP7UY$W(R;P` zXFpe(ziVE(v@sDF%;aZl&abb_8+Xfi+cm5@!UVaT;8w|M1J6*V)0NXS2s`P*H%FfD5CHtOQ?JjHrnM=`>_o`qXW6|;4ffoL zEukNWEmRgACg3pn24eiwxL>#lsvwLBvbg3pb99sICI_cPuWcO~16F`dcddlT1f`h9 znUBX;S$#mFO}iRXxY)?~U`qE@^@vr&V*7_Q+96WBC9VaMn%<`OTdTnlY9sczp5z@^ zb7y%duM!!COWS49C#2g4Y@NbGQH!f-dWbg@ub89KK{(}M9Gg{VV%eLayo{^88)vNM zS}oE+kuCyxslU@-ePwm$jO)~c7Ph7x;fNcAkaWHMSvYE zt8~yFr_rj+P{}qZ^5uCGr`?XUW!BM^AXUs@|5n+kgX-a$8P%l9 z&0FW8FRJ+DbAC`x%?i@#C>o;LXlNA8572 zMv_lkhG5Jwp0yV?I8;MpaE>0^L-n@dIcw+}E0j9H_eh>8XvdPUYP_bxM9MZ}5G3H$ z)Z^TnrLT-VY{d9P1!{H9`5tX1XhqA}*+vLjUGKm4;wVD+#T|)5OZQI{-s!wEMw!Z! zeuSM_FD#XVSuHV|AAq=T^bkuyYSMo4uznM7 z}CTQ|QH``4PU zuk3&?c03$a{ut=zS83L86Uw|V2T5->bvJN>MyKAj84tcX1$_~S+pe{1I<;O~63esv z!FeMiE%#mG(BM&nJpBWU;CLf|BU!+1oBHNiPPjFXPnQh!)WG}lP?1Ud!b6tHZl@8a z*O#jaRax+L^OtB<>0`qW3BqMxYO%_mM+zz!vtF3v=g_|3RWRyy%Cmt5wsli0kjFd% zjIwgtsm*RqDT-bl6tg}8U-o+27)Ca`F$Bv487IX)#_!yY7M}v&H41<1a_yCDYL=Y8 z5UlhA_bBnTZp#StP`Hi+=ycfn>HLrL1)1hASfGtL8Vh2T4-~_!R8OQgkgeR$lb^Z4 zM@~?a|3UjS&4Mna!j1(+x`L!U(o*ROVFM6)=GFMLSF}8yZK6RmZq(*nsOIPp?h?(R_?~+jB`tg2! zuz`~T{WsZ1;EM@Ddmnn@=3|$WNgX()Vo>VD^tg+K`cA!SKM={zab?4+OC(&W}BI0 zU+V#`-B}{9>N10)5p+t;rqt6JZ*FYtJa_u=gFSa`etm!M|KhWSw)HYQ_xNLxi!0A~ ztAxK%Q#jRqwDQgSflD5Q ze%T~WWHGSW1~h^#poC+i3ezN@^V{z)ynSJJdH$=*%lUzg*R3fVBm~%)fD4iAEzkxZ4i9m z{&H>f_GNW{f3?o_XuJ6QIEsXlL z$?1w{!xMKC9UkDomDlF9v%p5FSmJKsqoK#v`vMO}Yui=w(h0PHqDiJd##?yW_1Wyd z_LSsi$V{9L424JkR&V@oCOY}pzB?XQ3P0>wu?uv>j>eXpo0BXGWUK@q{AHc46X~=$ z{rs}b%gY`vMsqVAG{AjVRNr{y&i@&J8+wT>1PK&3vHgfHX&8OI!K?iv~cqV92 z$zdA>Jb>+DQ1=lVMTWrVoEbYVwgS)I+x5f2#GygZA$}2GgM$_D6vRsp+cz`2GMM)} zbUNP%1RkH(Gb>KzIV13>Lw<$Bt#&L0pD!HeRAOR!;SD_c(4!rA^q$q9g0cl5OEd~O zth0cY1YYibVgo#^?m1)14&(#=I;9wxUII^1^lq0w$0x#g$A(FjFVVdhxJD+cYT_Jr zpyf6VJmSd56$UapD98fOdU*Na@Xbc0hS}~5jwrra0d%dKul}UZ4ZyY;`-F#1@=QA_ zUp!_JWMurJn#I7e#2I)5o@||kbpX(@98)Y>WKEe~oL=$LwMY(V`DX^B803=*U3eK9 zOMnMBEIrJh&fWzKkw&5R4U63vY9&qgdYl6tG$H5E#*KXL;tJsLgnYnb6ia?QeB%H- zcTqXukwDoM;Ngmkmx-UO1Rf@Mj`>8vLVlqCz7#ffG%)-U0-hsz2^i?p`RwP&Y5*O} z;w5vaPm1wN>Xk1_mi$1=D;Xs9kdItk#710X{CL*L3_cYSco=|3d;su>N0ugFj3~G$ z`T)--gq>XpIv`Q@6-XgWP!Je9;*6ahpyMWCVxSW>U~J$4LoUG3jb%|$Vnq@I9diLQ z6sQrne_o(ea6*R&k{Cl^J1~}k`hoHcz_S=xIR4msg;>0v(|_zB0}yz+`njxgN@xNA D6L9|# literal 0 HcmV?d00001 diff --git a/docs/source/index.rst b/docs/source/index.rst index c881000f..0563b887 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -20,6 +20,7 @@ Documentation for ``obelisk`` logging.md joystick.rst obelisk_terminal_aliases.md + unitree_interfaces.md faq.md Overview diff --git a/docs/source/unitree_interfaces.md b/docs/source/unitree_interfaces.md new file mode 100644 index 00000000..ca1fc63b --- /dev/null +++ b/docs/source/unitree_interfaces.md @@ -0,0 +1,85 @@ +# Unitree Interfaces + +`Obelisk` provides interfaces to communicate with various robotic hardware, as well as simulation environments aimed to mimic hardware as closely as possible. +Currently supported systems include the Unitree Go2 quadruped and the Unitree G1 humanoid. + +## Finite State Machine + +The Unitree Interfaces implement a finite state machine (FSM) to facilate easier, safer testing. A downstream project may often want to implement a FSM of their own, which controls the flow of the downstream control. This FSM should be handled downstream; we present a fundamental FSM implementing key testing pieces such as an ESTOP, Damping mode, interaction with the default Unitree locomotion controllers, a User Pose, and the user defined controller. The FSM is given by the following graph: + +![Default FSM used in Unitree Interface](images/UnitreeFSM.png) + +## Controlling Robot Behavior via the Joystick + +Currently, the FSM and velocity commands can be sent to the robot through the joystick. By default, the joystick implements the following button behaviors: + + - Right Trigger: ESTOP + - Left Trigger: DAMPING + - Left Stick: Velocity Commands (vertical -> x, horizonal -> y) + - Right Stick: Yaw Rate Command + - Left Button: Layer 1 + +To avoid having the default FSM conflict with any user-specified FSM operating on the same controller, the default FSM is implemented on a different 'Layer' - the layer button must be held down, then the following buttons will trigger (when valid) transitions of the default FSM: + + - DPad Left (L1): USER POSE + - DPad Right (L1): UNITREE HOME + - DPad Up (L1): UNITREE VEL CTRL + - DPad Down (L1): USER CTRL + +where L1 refers to Layer 1, i.e. the layer button must be held down. + +All of these buttons can be remapped as desired in the setup yaml. Here is a complete yaml, with all available options (omitted options will take default values): + +``` +joystick: + on: True + pub_topic: /obelisk/g1/joy + sub_topic: /obelisk/g1/joy_feedback + ros_parameters: + v_x_scale: 0.5 + v_y_scale: 0.5 + w_z_scale: 0.5 + unitree_home_button: 116 // DR1 + user_pose_button: 127 // DL1 + low_level_ctrl_button: 128 // DD1 + high_level_ctrl_button: 117 // DU1 + damping_button: 12 // LT + estop: 15 // RT + vel_cmd_x: 11 // LY + vel_cmd_y: 10 // LX + vel_cmd_yaw: 14 // RY + layer: 4 // LB +``` +`scale` parameters refer to the multiplicative factor under which joystick inputs are scaled when passed to the unitree controller (or if the user controller is subscribed to the same message). +Buttons are passed as integers, under the following map: +``` +LT = 12 // 2 + AXIS_OFFSET, +RT = 15 // 5 + AXIS_OFFSET, +LB = 4, +RB = 5, +X = 3, +Y = 2, +A = 0, +B = 1, +DL = 27 // 6 + AXIS_OFFSET + NEG_OFFSET, +DR = 16 // 6 + AXIS_OFFSET, +DU = 17 // 7 + AXIS_OFFSET, +DD = 28 // 7 + AXIS_OFFSET + NEG_OFFSET, +LX = 10 // 0 + AXIS_OFFSET, +LY = 11 // 1 + AXIS_OFFSET, +RX = 13 // 3 + AXIS_OFFSET, +RY = 14, // 4 + AXIS_OFFSET, +MENU = 7, + +// Modified by layer button +X1 = 103 // X + LAYER_OFFSET, +Y1 = 102 // Y + LAYER_OFFSET, +A1 = 100 // A + LAYER_OFFSET, +B1 = 101 // B + LAYER_OFFSET, +DL1 = 127 // DL + LAYER_OFFSET, +DR1 = 116 // DR + LAYER_OFFSET, +DU1 = 117 // DU + LAYER_OFFSET, +DD1 = 128 // DD + LAYER_OFFSET, +``` + +Note that to avoid conflicts between `button` indices and `axis` indices (and issues with the DPad), we have added offsets `LAYER_OFFSET = 100`, to buttons on Layer 1, `AXIS_OFFSET = 10` to axes, and `NEG_OFFSET = 11` to elements of the DPad which report negative values when pressed. \ No newline at end of file From ee07e761f6bed0031bf36ab543d7a10a4e84687d Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 6 Nov 2025 16:01:01 -0800 Subject: [PATCH 04/27] joystick builds --- .../cpp/hardware/include/unitree_joystick.h | 247 ++++++++++-------- 1 file changed, 142 insertions(+), 105 deletions(-) diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index a95e194c..147e1d6e 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -1,3 +1,9 @@ +#include +#include +#include +#include +#include + #include "obelisk_controller.h" #include "obelisk_ros_utils.h" #include "sensor_msgs/msg/joy.hpp" @@ -11,6 +17,39 @@ namespace obelisk { class UnitreeJoystick : public ObeliskController { public: + static const int LAYER_OFFSET = 100; + static const int AXIS_OFFSET = 10; + static const int NEG_OFFSET = 11; + enum class ButtonMap : uint8_t { + LT = 2 + AXIS_OFFSET, + RT = 5 + AXIS_OFFSET, + LB = 4, + RB = 5, + X = 3, + Y = 2, + A = 0, + B = 1, + DL = 6 + AXIS_OFFSET + NEG_OFFSET, + DR = 6 + AXIS_OFFSET, + DU = 7 + AXIS_OFFSET, + DD = 7 + AXIS_OFFSET + NEG_OFFSET, + LX = 0 + AXIS_OFFSET, + LY = 1 + AXIS_OFFSET, + RX = 3 + AXIS_OFFSET, + RY = 4 + AXIS_OFFSET, + MENU = 7, + + // Modified by layer button + X1 = static_cast(X) + LAYER_OFFSET, + Y1 = static_cast(Y) + LAYER_OFFSET, + A1 = static_cast(A) + LAYER_OFFSET, + B1 = static_cast(B) + LAYER_OFFSET, + DL1 = static_cast(DL) + LAYER_OFFSET, + DR1 = static_cast(DR) + LAYER_OFFSET, + DU1 = static_cast(DU) + LAYER_OFFSET, + DD1 = static_cast(DD) + LAYER_OFFSET, + }; + UnitreeJoystick(const std::string& name) : ObeliskController(name) { // Set velocity bounds @@ -20,124 +59,124 @@ namespace obelisk { v_x_scale_ = this->get_parameter("v_x_scale").as_double(); v_y_scale_ = this->get_parameter("v_y_scale").as_double(); w_z_scale_ = this->get_parameter("w_z_scale").as_double(); + this->declare_parameter("axis_threshold", 15000); + axis_threshold_ = this->get_parameter("axis_threshold").as_int(); // Handle Execution FSM buttons // Buttons with negative values will corrospond to the DPAD - this->declare_parameter("unitree_home_button", ButtonMap::DR1); + this->declare_parameter("unitree_home_button", static_cast(ButtonMap::DR1)); unitree_home_button_ = this->get_parameter("unitree_home_button").as_int(); unitree_home_button_on_layer_ = isOnLayer(unitree_home_button_); unitree_home_button_on_axis_ = isOnAxis(unitree_home_button_); - unitree_home_button_on_dpad_ = isOnDpad(unitree_home_button); - if !recognizedButton(unitree_home_button) throw std::runtime_error("[UnitreeJoystick] Unitree Home Button " << unitree_home_button_ << " not recognized!!"); + unitree_home_button_on_dpad_ = isOnDpad(unitree_home_button_); + if (!recognizeButton(unitree_home_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree Home Button ") + std::to_string(unitree_home_button_) + std::string(" not recognized!!") + ); - this->declare_parameter("user_pose_button", ButtonMap::DL1); + this->declare_parameter("user_pose_button", static_cast(ButtonMap::DL1)); user_pose_button_ = this->get_parameter("user_pose_button").as_int(); user_pose_button_on_layer_ = isOnLayer(user_pose_button_); user_pose_button_on_axis_ = isOnAxis(user_pose_button_); user_pose_button_on_dpad_ = isOnDpad(user_pose_button_); - if !recognizedButton(user_pose_button_) throw std::runtime_error("[UnitreeJoystick] Unitree User Pose Button " << user_pose_button_ << " not recognized!!"); + if (!recognizeButton(user_pose_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree User Pose Button ") + std::to_string(user_pose_button_) + std::string(" not recognized!!") + ); - this->declare_parameter("low_level_ctrl_button", ButtonMap::DD1); + this->declare_parameter("low_level_ctrl_button", static_cast(ButtonMap::DD1)); low_level_ctrl_button_ = this->get_parameter("low_level_ctrl_button").as_int(); low_level_ctrl_button_on_layer_ = isOnLayer(low_level_ctrl_button_); low_level_ctrl_button_on_axis_ = isOnAxis(low_level_ctrl_button_); low_level_ctrl_button_on_dpad_ = isOnDpad(low_level_ctrl_button_); - if !recognizedButton(low_level_ctrl_button_) throw std::runtime_error("[UnitreeJoystick] Unitree Low Level Control Button " << low_level_ctrl_button_ << " not recognized!!"); + if (!recognizeButton(low_level_ctrl_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree Low Level Control Button ") + std::to_string(low_level_ctrl_button_) + std::string(" not recognized!!") + ); - this->declare_parameter("high_level_ctrl_button", ButtonMap::DU1); + this->declare_parameter("high_level_ctrl_button", static_cast(ButtonMap::DU1)); high_level_ctrl_button_ = this->get_parameter("high_level_ctrl_button").as_int(); high_level_ctrl_button_on_layer_ = isOnLayer(high_level_ctrl_button_); high_level_ctrl_button_on_axis_ = isOnAxis(high_level_ctrl_button_); high_level_ctrl_button_on_dpad_ = isOnDpad(high_level_ctrl_button_); - if !recognizedButton(high_level_ctrl_button_) throw std::runtime_error("[UnitreeJoystick] Unitree High Level Control Button " << high_level_ctrl_button_ << " not recognized!!"); + if (!recognizeButton(high_level_ctrl_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree High Level Control Button ") + std::to_string(high_level_ctrl_button_) + std::string(" not recognized!!") + ); - this->declare_parameter("damping_button", ButtonMap::LT); + this->declare_parameter("damping_button", static_cast(ButtonMap::LT)); damping_button_ = this->get_parameter("damping_button").as_int(); damping_button_on_layer_ = isOnLayer(damping_button_); damping_button_on_axis_ = isOnAxis(damping_button_); damping_button_on_dpad_ = isOnDpad(damping_button_); - if !recognizedButton(damping_button_) throw std::runtime_error("[UnitreeJoystick] Unitree Damping Button " << damping_button_ << " not recognized!!"); + if (!recognizeButton(damping_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree Damping Button ") + std::to_string(damping_button_) + std::string(" not recognized!!") + ); - this->declare_parameter("estop", ButtonMap::RT); + this->declare_parameter("estop", static_cast(ButtonMap::RT)); estop_ = this->get_parameter("estop").as_int(); estop_on_layer_ = isOnLayer(estop_); estop_on_axis_ = isOnAxis(estop_); estop_on_dpad_ = isOnDpad(estop_); - if !recognizedButton(estop_) throw std::runtime_error("[UnitreeJoystick] Unitree ESTOP Button " << estop_ << " not recognized!!"); + if (!recognizeButton(estop_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree ESTOP Button ") + std::to_string(estop_) + std::string(" not recognized!!") + ); - this->declare_parameter("vel_cmd_x", ButtonMap::LY); + this->declare_parameter("vel_cmd_x", static_cast(ButtonMap::LY)); vel_cmd_x_ = this->get_parameter("vel_cmd_x").as_int(); vel_cmd_x_on_layer_ = isOnLayer(vel_cmd_x_); vel_cmd_x_on_axis_ = isOnAxis(vel_cmd_x_); vel_cmd_x_on_dpad_ = isOnDpad(vel_cmd_x_); - if !recognizedButton(vel_cmd_x_) throw std::runtime_error("[UnitreeJoystick] Unitree x vel axis " << vel_cmd_x_ << " not recognized!!"); - if !vel_cmd_x_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree x vel axis " << vel_cmd_x_ << " is not on a joystick axis!!"); + if (!recognizeButton(vel_cmd_x_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree x vel axis ") + std::to_string(vel_cmd_x_) + std::string(" not recognized!!") + ); + if (!vel_cmd_x_on_axis_) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree x vel axis ") + std::to_string(vel_cmd_x_) + std::string(" is not on a joystick axis!!") + ); - this->declare_parameter("vel_cmd_y", ButtonMap::LX); + this->declare_parameter("vel_cmd_y", static_cast(ButtonMap::LX)); vel_cmd_y_ = this->get_parameter("vel_cmd_y").as_int(); vel_cmd_y_on_layer_ = isOnLayer(vel_cmd_y_); vel_cmd_y_on_axis_ = isOnAxis(vel_cmd_y_); vel_cmd_y_on_dpad_ = isOnDpad(vel_cmd_y_); - if !recognizedButton(vel_cmd_y_) throw std::runtime_error("[UnitreeJoystick] Unitree y vel axis " << vel_cmd_y_ << " not recognized!!"); - if !vel_cmd_y_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree y vel axis " << vel_cmd_y_ << " is not on a joystick axis!!"); + if (!recognizeButton(vel_cmd_y_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree y vel axis ") + std::to_string(vel_cmd_y_) + std::string(" not recognized!!") + ); + if (!vel_cmd_y_on_axis_) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree y vel axis ") + std::to_string(vel_cmd_y_) + std::string(" is not on a joystick axis!!") + ); - this->declare_parameter("vel_cmd_yaw", ButtonMap::RY); + this->declare_parameter("vel_cmd_yaw", static_cast(ButtonMap::RY)); vel_cmd_yaw_ = this->get_parameter("vel_cmd_yaw").as_int(); vel_cmd_yaw_on_layer_ = isOnLayer(vel_cmd_yaw_); vel_cmd_yaw_on_axis_ = isOnAxis(vel_cmd_yaw_); vel_cmd_yaw_on_dpad_ = isOnDpad(vel_cmd_yaw_); - if !recognizedButton(vel_cmd_yaw_) throw std::runtime_error("[UnitreeJoystick] Unitree yaw vel axis " << vel_cmd_yaw_ << " not recognized!!"); - if !vel_cmd_yaw_on_axis_ throw std::runtime_error("[UnitreeJoystick] Unitree yaw vel axis " << vel_cmd_yaw_ << " is not on a joystick axis!!"); + if (!recognizeButton(vel_cmd_yaw_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree yaw vel axis ") + std::to_string(vel_cmd_yaw_) + std::string(" not recognized!!") + ); + if (!vel_cmd_yaw_on_axis_) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree yaw vel axis ") + std::to_string(vel_cmd_yaw_) + std::string(" is not on a joystick axis!!") + ); - this->declare_parameter("layer_button", ButtonMap::LB); - layer_button = this->get_parameter("layer_button"); - layer_buttonon_axis_ = isOnAxis(layer_button); - layer_buttonon_dpad_ = isOnDpad(layer_button); - if !recognizedButton(layer_button) throw std::runtime_error("[UnitreeJoystick] Unitree Layer Button " << layer_button << " not recognized!!"); + this->declare_parameter("layer_button", static_cast(ButtonMap::LB)); + layer_button_ = this->get_parameter("layer_button").as_int(); + layer_button_on_axis_ = isOnAxis(layer_button_); + layer_button_on_dpad_ = isOnDpad(layer_button_); + if (!recognizeButton(layer_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Unitree Layer Button ") + std::to_string(layer_button_) + std::string(" not recognized!!") + ); - std::initializer_list values = {estop_, damping_button_, user_pose_button_, unitree_home_button_, low_level_ctrl_button_, high_level_ctrl_button_, vel_cmd_x_, vel_cmd_y_, vel_cmd_yaw_, layer_button}; + std::initializer_list values = {estop_, damping_button_, user_pose_button_, unitree_home_button_, low_level_ctrl_button_, high_level_ctrl_button_, vel_cmd_x_, vel_cmd_y_, vel_cmd_yaw_, layer_button_}; std::unordered_set s(values.begin(), values.end()); - if !(s.size() == values.size()) throw std::runtime_error("[UnitreeJoystick] Unitree Joystick Configuration contains duplicate buttons " << values); + if (!(s.size() == values.size())) throw std::runtime_error( + "[UnitreeJoystick] Duplicate buttons: set size " + + std::to_string(s.size()) + " vs values " + + std::to_string(values.size()) + ); // Register publishers this->RegisterObkPublisher("pub_exec_fsm_setting", pub_exec_fsm_key_); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick node has been initialized."); } - static const int LAYER_OFFSET = 100; - static const int AXIS_OFFSET = 10; - static const int NEG_OFFSET = 11; - static const enum class ButtonMap : uint8_t { - LT = 2 + AXIS_OFFSET, - RT = 5 + AXIS_OFFSET, - LB = 4, - RB = 5, - X = 3, - Y = 2, - A = 0, - B = 1, - DL = 6 + AXIS_OFFSET + NEG_OFFSET, - DR = 6 + AXIS_OFFSET, - DU = 7 + AXIS_OFFSET, - DD = 7 + AXIS_OFFSET + NEG_OFFSET, - LX = 0 + AXIS_OFFSET, - LY = 1 + AXIS_OFFSET, - RX = 3 + AXIS_OFFSET, - RY = 4 + AXIS_OFFSET, - MENU = 7, - - // Modified by layer button - X1 = X + LAYER_OFFSET, - Y1 = Y + LAYER_OFFSET, - A1 = A + LAYER_OFFSET, - B1 = B + LAYER_OFFSET, - DL1 = DL + LAYER_OFFSET, - DR1 = DR + LAYER_OFFSET, - DU1 = DU + LAYER_OFFSET, - DD1 = DD + LAYER_OFFSET, - }; - - static const std::unordered_map ButtonNames = { + + inline static const std::unordered_map BUTTON_NAMES = { {ButtonMap::LT, "Left Trigger"}, {ButtonMap::RT, "Right Trigger"}, {ButtonMap::LB, "Left Button"}, @@ -169,30 +208,27 @@ namespace obelisk { void UpdateXHat(__attribute__((unused)) const sensor_msgs::msg::Joy& msg) override { vel_cmd_msg vel_msg; vel_msg.header.stamp = this->now(); - vel_msg.v_x = msg.axes[ButtonMap::LY] * v_x_scale_; - vel_msg.v_y = msg.axes[ButtonMap::LX] * v_y_scale_; - vel_msg.w_z = msg.axes[ButtonMap::RX] * w_z_scale_; + vel_msg.v_x = msg.axes[static_cast(ButtonMap::LY)] * v_x_scale_; + vel_msg.v_y = msg.axes[static_cast(ButtonMap::LX)] * v_y_scale_; + vel_msg.w_z = msg.axes[static_cast(ButtonMap::RX)] * w_z_scale_; this->GetPublisher(this->ctrl_key_)->publish(vel_msg); - - // ----- Axes ----- // - constexpr int DPAD_VERTICAL = 7; - constexpr int DPAD_HORIZONTAL = 6; // Print control menu static rclcpp::Time last_menu_press = this->now(); - if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[ButtonMap::MENU]) { + if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[static_cast(ButtonMap::MENU)]) { RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick Button Layout (XBox Controller):\n" << "Execution Finite State Machine:\n" << - "\tHOME: " << ButtonNames[unitree_home_button_] << "\n" << - "\tDAMPING: " << ButtonNames[damping_button_] << "\n" << - "\tLOW_LEVEL_CTRL: " << ButtonNames[low_level_ctrl_button_] << "\n" << - "\tHIGH_LEVEL_CTRL: " << ButtonNames[high_level_ctrl_button_] << "\n" << + "\tHOME: " << BUTTON_NAMES.at(static_cast(unitree_home_button_)) << "\n" << + "\tDAMPING: " << BUTTON_NAMES.at(static_cast(damping_button_)) << "\n" << + "\tLOW_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(low_level_ctrl_button_)) << "\n" << + "\tHIGH_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(high_level_ctrl_button_)) << "\n" << "High Level Velocity Commands:\n" << - "\tFront/Back: " << ButtonNames[vel_cmd_x_] << "\n" << - "\tSide/Side: " << ButtonNames[vel_cmd_y_] << "\n" << - "\tYaw Rate: " << ButtonNames[vel_cmd_yaw_] << ""; + "\tFront/Back: " << BUTTON_NAMES.at(static_cast(vel_cmd_x_)) << "\n" << + "\tSide/Side: " << BUTTON_NAMES.at(static_cast(vel_cmd_y_)) << "\n" << + "\tYaw Rate: " << BUTTON_NAMES.at(static_cast(vel_cmd_yaw_)) + ); last_menu_press = this->now(); } @@ -202,29 +238,29 @@ namespace obelisk { unitree_fsm_msg fsm_msg; fsm_msg.header.stamp = this->now(); - if getEstop(msg) { + if (getEstopButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // ESTOP - } else if getDamping(msg) { + } else if (getDampingButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // Damping - } else if getUserPose(msg) { + } else if (getUserPoseButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // User Pose - } else if getUnitreeHome(msg) { + } else if (getUnitreeHomeButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // Unitree Stand - } else if getUserCtrl(msg) { + } else if (getUserCtrlButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // User Control - } else if getUnitreeCtrl(msg) { + } else if (getUnitreeCtrlButton(msg)) { fsm_msg.cmd_exec_fsm_state = 5; // Unitree Control } else { - continue; + return; } - last_Dpad_press = this->now(); + last_fsm_msg = this->now(); this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command."); } } - bool recognizeButton(ButtonMap btn) { - return ButtonNames.count(btn) > 0 + bool recognizeButton(int btn) { + return BUTTON_NAMES.count(static_cast(btn)) > 0; } bool getEstopButton(const sensor_msgs::msg::Joy& msg) { @@ -244,43 +280,43 @@ namespace obelisk { } bool getUserCtrlButton(const sensor_msgs::msg::Joy& msg) { - return getButton(msg, low_level_ctrl_button_, low_level_ctrl_button_on_layer_, low_level_ctrl_button_on_axis_, low_level_ctrl_on_dpad_); + return getButton(msg, low_level_ctrl_button_, low_level_ctrl_button_on_layer_, low_level_ctrl_button_on_axis_, low_level_ctrl_button_on_dpad_); } bool getUnitreeCtrlButton(const sensor_msgs::msg::Joy& msg) { return getButton(msg, high_level_ctrl_button_, high_level_ctrl_button_on_layer_, high_level_ctrl_button_on_axis_, high_level_ctrl_button_on_dpad_); } - bool getButton(const sensor_msgs::msg::Joy& msg, ButtonMap btn, bool on_layer, bool on_axis, bool on_dpad){ - if on_layer { - if !get_button(msg, layer_button, false, layer_on_axis, layer_on_dpad) { + bool getButton(const sensor_msgs::msg::Joy& msg, int btn, bool on_layer, bool on_axis, bool on_dpad){ + if (on_layer) { + if (!getButton(msg, layer_button_, false, layer_button_on_axis_, layer_button_on_dpad_)) { return false; } btn -= LAYER_OFFSET; } - if on_axis { - return msg.axis[btn - AXIS_OFFSET] > axis_threshold_; + if (on_axis) { + return msg.axes[btn - AXIS_OFFSET] > axis_threshold_; } - if on_dpad { + if (on_dpad) { int sgn = 1; - if (btn == ButtonMap::DL || btn == ButtonMap::DD || btn == ButtonMap::DL1 || btn == ButtonMap::DD1) { + if (btn == static_cast(ButtonMap::DL) || btn == static_cast(ButtonMap::DD) || btn == static_cast(ButtonMap::DL1) || btn == static_cast(ButtonMap::DD1)) { btn -= NEG_OFFSET; sgn = -1; } - return msg.axis[btn - AXIS_OFFSET] * sgn > 0.5 + return msg.axes[btn - AXIS_OFFSET] * sgn > 0.5; } return msg.buttons[btn]; } - bool isOnLayer(ButtonMap btn) { - return btn == ButtonMap::X1 || btn == ButtonMap::Y1 || btn == ButtonMap::A1 || btn == ButtonMap::B1 || btn == ButtonMap::DL1 || btn == ButtonMap::DR1 || btn == ButtonMap::DU1 || btn == ButtonMap::DD1; + bool isOnLayer(int btn) { + return btn == static_cast(ButtonMap::X1) || btn == static_cast(ButtonMap::Y1) || btn == static_cast(ButtonMap::A1) || btn == static_cast(ButtonMap::B1) || btn == static_cast(ButtonMap::DL1) || btn == static_cast(ButtonMap::DR1) || btn == static_cast(ButtonMap::DU1) || btn == static_cast(ButtonMap::DD1); } - bool isOnAxis(ButtonMap btn) { - return btn == ButtonMap::LT || btn == ButtonMap::RT || btn == ButtonMap::LX || btn == ButtonMap::LY || btn == ButtonMap::RX || btn == ButtonMap::RY; + bool isOnAxis(int btn) { + return btn == static_cast(ButtonMap::LT) || btn == static_cast(ButtonMap::RT) || btn == static_cast(ButtonMap::LX) || btn == static_cast(ButtonMap::LY) || btn == static_cast(ButtonMap::RX) || btn == static_cast(ButtonMap::RY); } - bool isOnDpad(ButtonMap btn) { - return btn == ButtonMap::DL || btn == ButtonMap::DR || btn == ButtonMap::DU || btn == ButtonMap::DD || btn == ButtonMap::DL1 || btn == ButtonMap::DR1 || btn == ButtonMap::DU1 || btn == ButtonMap::DD1; + bool isOnDpad(int btn) { + return btn == static_cast(ButtonMap::DL) || btn == static_cast(ButtonMap::DR) || btn == static_cast(ButtonMap::DU) || btn == static_cast(ButtonMap::DD) || btn == static_cast(ButtonMap::DL1) || btn == static_cast(ButtonMap::DR1) || btn == static_cast(ButtonMap::DU1) || btn == static_cast(ButtonMap::DD1); } vel_cmd_msg ComputeControl() override { @@ -297,6 +333,7 @@ namespace obelisk { float v_x_scale_; float v_y_scale_; float w_z_scale_; + uint32_t axis_threshold_; // Hold button locations for execution fsm int damping_button_; @@ -305,7 +342,7 @@ namespace obelisk { int low_level_ctrl_button_; int high_level_ctrl_button_; int estop_; - int layer_button; + int layer_button_; bool damping_button_on_layer_; bool unitree_home_button_on_layer_; bool user_pose_button_on_layer_; @@ -324,7 +361,7 @@ namespace obelisk { bool low_level_ctrl_button_on_dpad_; bool high_level_ctrl_button_on_dpad_; bool layer_button_on_dpad_; - bool layer_button_on_axis; + bool layer_button_on_axis_; bool estop_on_dpad_; int vel_cmd_x_; From 8299ba850e90bbf60814a95c4492e1655278b51b Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 10:37:06 -0800 Subject: [PATCH 05/27] joystick launch changes --- .../obelisk_py/core/utils/launch_utils.py | 42 +++++------- .../src/obelisk_ros/config/dummy_cpp.yaml | 1 + obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 64 +++++++++---------- 3 files changed, 48 insertions(+), 59 deletions(-) diff --git a/obelisk/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index 2bb8e818..5953314b 100644 --- a/obelisk/python/obelisk_py/core/utils/launch_utils.py +++ b/obelisk/python/obelisk_py/core/utils/launch_utils.py @@ -347,27 +347,22 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None return launch_actions +JOYSTICK_PARAMS = ["device_id", "device_name", "deadzone", "autorepeat_rate", "sticky_buttons", "coalesce_interval_ms"] +JOYSTICK_REMAPPINGS = {"pub_topic": "\joy", "sub_topic": "/joy/set_feedback"} + def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: LifecycleNode) -> List[LifecycleNode]: """Gets and configures all the launch actions related to joystick given the settings from the yaml.""" launch_actions = [] if settings["on"]: - # TODO: I think there is a better way to handle these defaults, i.e., not even passing the parameter if nothing - # is supplied. - device_id = settings["device_id"] if "device_id" in settings else 0 - - device_name = settings["device_name"] if "device_name" in settings else "" - - deadzone = settings["deadzone"] if "deadzone" in settings else 0.05 - - autorepeat_rate = settings["autorepeat_rate"] if "autorepeat_rate" in settings else 20.0 - - sticky_buttons = settings["sticky_buttons"] if "sticky_buttons" in settings else False - - coalesce_interval_ms = settings["coalesce_interval_ms"] if "coalesce_interval_ms" in settings else 1 - - pub_topic = settings["pub_topic"] if "pub_topic" in settings else "/joy" - - sub_topic = settings["sub_topic"] if "sub_topic" in settings else "/joy/set_feedback" + joystick_params = {} + joystick_remappings = JOYSTICK_REMAPPINGS.copy() + for setting_key, setting_val in settings.items(): + if setting_key in JOYSTICK_PARAMS: + joystick_params[setting_key] = setting_val + elif setting_key in JOYSTICK_REMAPPINGS.keys(): + joystick_remappings[setting_key] = setting_val + else: + raise ValueError(f"Unknown joystick setting {setting_key}") launch_actions += [ Node( @@ -376,23 +371,16 @@ def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: name="joy", output="screen", parameters=[ - { - "device_id": device_id, - "device_name": device_name, - "deadzone": deadzone, - "autorepeat_rate": autorepeat_rate, - "sticky_buttons": sticky_buttons, - "coalesce_interval_ms": coalesce_interval_ms, - } + joystick_params ], remappings=[ ( "/joy", - pub_topic, + joystick_remappings["pub_topic"], ), # remap the topic that the joystick publishes to ( "/joy/set_feedback", - sub_topic, + joystick_remappings["sub_topic"], ), # remap the topic where the joystick listens ], ) diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index 76a68c53..15b33735 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml @@ -117,3 +117,4 @@ onboard: on: True pub_topic: /obelisk/dummy/joy sub_topic: /obelisk/dummy/joy_feedback + asdf: True diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index bac0fdb9..e6fdaf45 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -68,40 +68,40 @@ onboard: callback_group: None # sensing: robot: - # # === simulation === - # - is_simulated: True - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_sim - # params: - # ic_keyframe: standing - # === hardware === - - is_simulated: False + # === simulation === + - is_simulated: True pkg: obelisk_unitree_cpp - executable: obelisk_unitree_g1_hardware + executable: obelisk_unitree_sim params: - network_interface_name: enp4s0 - default_kp: [ - 10., 10., 10., # L hip - 10., 10., 10., # L knee, ankle - 10., 10., 10., # R hip - 10., 10., 10., # R knee, ankle - 10., #10., 10., # Waist - 5., 5., 5., # L shoulder - 5., 5., 5., 5., # L elbow, wrist - 5., 5., 5., # R shoulder - 5., 5., 5., 5., # R elbow, wrist - ] - default_kd: [ - 1., 1., 1., # L hip - 1., 1., 1., # L knee, ankle - 1., 1., 1., # R hip - 1., 1., 1., # R knee, ankle - 1., #1., 1., # Waist - 1., 1., 1., # L shoulder - 1., 1., 1., 1., # L elbow, wrist - 1., 1., 1., # R shoulder - 1., 1., 1., 1., # R elbow, wrist - ] + ic_keyframe: standing + # === hardware === + # - is_simulated: False + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_g1_hardware + # params: + # network_interface_name: enp4s0 + # default_kp: [ + # 10., 10., 10., # L hip + # 10., 10., 10., # L knee, ankle + # 10., 10., 10., # R hip + # 10., 10., 10., # R knee, ankle + # 10., #10., 10., # Waist + # 5., 5., 5., # L shoulder + # 5., 5., 5., 5., # L elbow, wrist + # 5., 5., 5., # R shoulder + # 5., 5., 5., 5., # R elbow, wrist + # ] + # default_kd: [ + # 1., 1., 1., # L hip + # 1., 1., 1., # L knee, ankle + # 1., 1., 1., # R hip + # 1., 1., 1., # R knee, ankle + # 1., #1., 1., # Waist + # 1., 1., 1., # L shoulder + # 1., 1., 1., 1., # L elbow, wrist + # 1., 1., 1., # R shoulder + # 1., 1., 1., 1., # R elbow, wrist + # ] # ================== # callback_groups: publishers: From e1ca256c99a368e18a5290ad1b47edb67c14b158 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 11:00:23 -0800 Subject: [PATCH 06/27] joystick launch throws errors --- .../obelisk_py/core/utils/launch_utils.py | 34 +++++++++++++------ .../src/obelisk_ros/config/dummy_cpp.yaml | 1 - 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/obelisk/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index 5953314b..f6236dbc 100644 --- a/obelisk/python/obelisk_py/core/utils/launch_utils.py +++ b/obelisk/python/obelisk_py/core/utils/launch_utils.py @@ -317,17 +317,27 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None if "viz_tool" not in settings or settings["viz_tool"] == "rviz": # Setup Rviz - rviz_file_name = "rviz/" + settings["rviz_config"] - rviz_config_path = os.path.join(get_package_share_directory(settings["rviz_pkg"]), rviz_file_name) - launch_actions += [ - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", rviz_config_path], - ) - ] + if "rviz_config" not in settings.keys(): + launch_actions += [ + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + ) + ] + else: + rviz_file_name = "rviz/" + settings["rviz_config"] + rviz_config_path = os.path.join(get_package_share_directory(settings["rviz_pkg"]), rviz_file_name) + launch_actions += [ + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_config_path], + ) + ] elif settings["viz_tool"] == "foxglove": # setup fox glove xml_launch_file = os.path.join( @@ -357,6 +367,8 @@ def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: joystick_params = {} joystick_remappings = JOYSTICK_REMAPPINGS.copy() for setting_key, setting_val in settings.items(): + if setting_key == "on": + continue if setting_key in JOYSTICK_PARAMS: joystick_params[setting_key] = setting_val elif setting_key in JOYSTICK_REMAPPINGS.keys(): diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index 15b33735..76a68c53 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml @@ -117,4 +117,3 @@ onboard: on: True pub_topic: /obelisk/dummy/joy sub_topic: /obelisk/dummy/joy_feedback - asdf: True From fe1dfc45d22c55c956b66cdf3a2c4ffffd39fe23 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 11:49:10 -0800 Subject: [PATCH 07/27] unitree joystick functional --- obelisk/cpp/hardware/include/unitree_fsm.h | 13 +++++ .../cpp/hardware/include/unitree_joystick.h | 53 ++++++++++++------- .../hardware/robots/unitree/CMakeLists.txt | 2 +- .../robots/unitree/unitree_interface.h | 15 +----- .../obelisk_py/core/utils/launch_utils.py | 2 +- .../obelisk_ros/config/unitree_fsm_debug.yaml | 30 +++++++++++ .../launch/obelisk_bringup.launch.py | 40 +++++++------- 7 files changed, 101 insertions(+), 54 deletions(-) create mode 100644 obelisk/cpp/hardware/include/unitree_fsm.h create mode 100644 obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml diff --git a/obelisk/cpp/hardware/include/unitree_fsm.h b/obelisk/cpp/hardware/include/unitree_fsm.h new file mode 100644 index 00000000..a92e2875 --- /dev/null +++ b/obelisk/cpp/hardware/include/unitree_fsm.h @@ -0,0 +1,13 @@ +#pragma once +#include + + +enum class ExecFSMState : uint8_t { + INIT = 0, + UNITREE_HOME = 1, + USER_POSE = 2, + USER_CTRL = 3, + UNITREE_VEL_CTRL = 4, + DAMPING = 5, + ESTOP = 6 +}; \ No newline at end of file diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index 147e1d6e..e876fe67 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -9,6 +9,7 @@ #include "sensor_msgs/msg/joy.hpp" #include "obelisk_control_msgs/msg/execution_fsm.hpp" #include "obelisk_control_msgs/msg/velocity_command.hpp" +#include "unitree_fsm.h" namespace obelisk { @@ -59,8 +60,8 @@ namespace obelisk { v_x_scale_ = this->get_parameter("v_x_scale").as_double(); v_y_scale_ = this->get_parameter("v_y_scale").as_double(); w_z_scale_ = this->get_parameter("w_z_scale").as_double(); - this->declare_parameter("axis_threshold", 15000); - axis_threshold_ = this->get_parameter("axis_threshold").as_int(); + this->declare_parameter("axis_threshold", -0.1); + axis_threshold_ = this->get_parameter("axis_threshold").as_double(); // Handle Execution FSM buttons // Buttons with negative values will corrospond to the DPAD @@ -233,30 +234,42 @@ namespace obelisk { } // Trigger FSM + // First, check for damping or estop + bool commanded = false; + unitree_fsm_msg fsm_msg; + fsm_msg.header.stamp = this->now(); + if (getEstopButton(msg)) { + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::ESTOP); // ESTOP + commanded = true; + } else if (getDampingButton(msg)) { + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::DAMPING); // Damping + commanded = true; + } + static rclcpp::Time last_fsm_msg = this->now(); - if ((this->now() - last_fsm_msg).seconds() > 1) { - unitree_fsm_msg fsm_msg; - fsm_msg.header.stamp = this->now(); - - if (getEstopButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // ESTOP - } else if (getDampingButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // Damping - } else if (getUserPoseButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // User Pose + if ((this->now() - last_fsm_msg).seconds() > 0.5) { + if (getUserPoseButton(msg)) { + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::USER_POSE); // User Pose + commanded = true; } else if (getUnitreeHomeButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // Unitree Stand + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::UNITREE_HOME); // Unitree Stand + commanded = true; } else if (getUserCtrlButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // User Control + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::USER_CTRL); // User Control + commanded = true; } else if (getUnitreeCtrlButton(msg)) { - fsm_msg.cmd_exec_fsm_state = 5; // Unitree Control - } else { - return; + fsm_msg.cmd_exec_fsm_state = static_cast(ExecFSMState::UNITREE_VEL_CTRL); // Unitree Control + commanded = true; + } + if (commanded) { + last_fsm_msg = this->now(); } - last_fsm_msg = this->now(); + } + if (commanded){ this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command."); } + } bool recognizeButton(int btn) { @@ -294,7 +307,7 @@ namespace obelisk { btn -= LAYER_OFFSET; } if (on_axis) { - return msg.axes[btn - AXIS_OFFSET] > axis_threshold_; + return msg.axes[btn - AXIS_OFFSET] < axis_threshold_; } if (on_dpad) { int sgn = 1; @@ -333,7 +346,7 @@ namespace obelisk { float v_x_scale_; float v_y_scale_; float w_z_scale_; - uint32_t axis_threshold_; + float axis_threshold_; // Hold button locations for execution fsm int damping_button_; diff --git a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt index f01a7878..f614f2fa 100644 --- a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt +++ b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt @@ -17,7 +17,7 @@ set(BUILD_EXAMPLES OFF CACHE BOOL "Disable building examples") FetchContent_MakeAvailable(unitree_sdk) add_library(UnitreeInterface INTERFACE) -target_include_directories(UnitreeInterface INTERFACE .) +target_include_directories(UnitreeInterface INTERFACE . ${CMAKE_CURRENT_SOURCE_DIR}/../../include) target_link_libraries(UnitreeInterface INTERFACE Obelisk::Core unitree_sdk2) diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index ec5d054b..ed8f01e7 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -1,4 +1,5 @@ # pragma once +#include "unitree_fsm.h" #include "obelisk_robot.h" #include "obelisk_ros_utils.h" #include "obelisk_sensor_msgs/msg/obk_joint_encoders.h" @@ -11,13 +12,10 @@ #include #include - // DDS #include #include -// IDL -#include // Get rid of this?? namespace obelisk { using unitree_control_msg = obelisk_control_msgs::msg::PDFeedForward; @@ -100,16 +98,6 @@ namespace obelisk { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - enum class ExecFSMState : uint8_t { - INIT = 0, - UNITREE_HOME = 1, - USER_POSE = 2, - USER_CTRL = 3, - UNITREE_VEL_CTRL = 4, - DAMPING = 5, - ESTOP = 6 - }; - const std::unordered_map TRANSITION_STRINGS = { {ExecFSMState::INIT, "INIT"}, {ExecFSMState::UNITREE_HOME, "UNITREE_HOME"}, @@ -270,7 +258,6 @@ namespace obelisk { std::string STATE_TOPIC_; std::string network_interface_name_; - std::shared_ptr mode_switch_manager_; size_t num_motors_; std::vector joint_names_; diff --git a/obelisk/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index f6236dbc..a6739984 100644 --- a/obelisk/python/obelisk_py/core/utils/launch_utils.py +++ b/obelisk/python/obelisk_py/core/utils/launch_utils.py @@ -358,7 +358,7 @@ def _single_viz_node_launch_actions(settings: Dict, suffix: Optional[int] = None JOYSTICK_PARAMS = ["device_id", "device_name", "deadzone", "autorepeat_rate", "sticky_buttons", "coalesce_interval_ms"] -JOYSTICK_REMAPPINGS = {"pub_topic": "\joy", "sub_topic": "/joy/set_feedback"} +JOYSTICK_REMAPPINGS = {"pub_topic": "/joy", "sub_topic": "/joy/set_feedback"} def get_launch_actions_from_joystick_settings(settings: Dict, global_state_node: LifecycleNode) -> List[LifecycleNode]: """Gets and configures all the launch actions related to joystick given the settings from the yaml.""" diff --git a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml new file mode 100644 index 00000000..37b9ad72 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml @@ -0,0 +1,30 @@ +config: unitree_fsm_debug +onboard: + control: + # ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/fsm_test/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/fsm_test/high_level_ctrl + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/fsm_test/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 100 # Control callback is not being used + callback_group: None + joystick: + on: True + pub_topic: /obelisk/fsm_test/joy + sub_topic: /obelisk/fsm_test/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py index 853e4954..d5a5c337 100644 --- a/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py +++ b/obelisk_ws/src/obelisk_ros/launch/obelisk_bringup.launch.py @@ -69,9 +69,10 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: logger.info(f"Bringing up the Obelisk nodes on device {device_name}...") # checks - we must at least have these 3 components - assert "control" in obelisk_config - assert "estimation" in obelisk_config - assert "robot" in obelisk_config + # New -> no longer require control/estimation/robot + # assert "control" in obelisk_config + # assert "estimation" in obelisk_config + # assert "robot" in obelisk_config obelisk_launch_actions = [] # Setup logging @@ -133,21 +134,24 @@ def obelisk_setup(context: launch.LaunchContext, launch_args: Dict) -> List: obelisk_launch_actions += [configure_event] # generate all launch actions - obelisk_launch_actions += get_launch_actions_from_node_settings( - obelisk_config["control"], - "control", - global_state_node, - ) - obelisk_launch_actions += get_launch_actions_from_node_settings( - obelisk_config["estimation"], - "estimation", - global_state_node, - ) - obelisk_launch_actions += get_launch_actions_from_node_settings( - obelisk_config["robot"], - "robot", - global_state_node, - ) + if "control" in obelisk_config: + obelisk_launch_actions += get_launch_actions_from_node_settings( + obelisk_config["control"], + "control", + global_state_node, + ) + if "estimation" in obelisk_config: + obelisk_launch_actions += get_launch_actions_from_node_settings( + obelisk_config["estimation"], + "estimation", + global_state_node, + ) + if "robot" in obelisk_config: + obelisk_launch_actions += get_launch_actions_from_node_settings( + obelisk_config["robot"], + "robot", + global_state_node, + ) if "sensing" in obelisk_config: obelisk_launch_actions += get_launch_actions_from_node_settings( obelisk_config["sensing"], From 05d259d7b585fe9cece05ca8b27495a64976c580 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 15:53:47 -0800 Subject: [PATCH 08/27] motor logging compiles --- .../hardware/robots/unitree/g1_interface.h | 73 ++++++----------- .../hardware/robots/unitree/go2_interface.h | 69 +++++++++++++++- .../robots/unitree/unitree_interface.h | 79 +++++++++++++------ obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 2 +- 4 files changed, 144 insertions(+), 79 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 22069290..adf20d47 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -6,13 +6,11 @@ #include // IDL -#include #include +#include #include -#include #include -#include #include #include @@ -21,10 +19,10 @@ namespace obelisk { using namespace unitree_hg::msg::dds_; - class G1Interface : public ObeliskUnitreeInterface { + class G1Interface : public ObeliskUnitreeInterface { public: G1Interface(const std::string& node_name) - : ObeliskUnitreeInterface(node_name), use_hands_(false), fixed_waist_(true), mode_pr_(AnkleMode::PR), mode_machine_(0) { + : ObeliskUnitreeInterface(node_name, "g1"), use_hands_(false), fixed_waist_(true), mode_pr_(AnkleMode::PR), mode_machine_(0) { this->declare_parameter("use_hands", false); use_hands_ = this->get_parameter("use_hands").as_bool(); @@ -71,7 +69,6 @@ namespace obelisk { CMD_TOPIC_ = "rt/lowcmd"; STATE_TOPIC_ = "rt/lowstate"; - MAIN_BOARD_STATE_TOPIC_ = "rt/lf/mainboardstate"; // TODO: Add support for rt/lowstate_doubleimu @@ -84,12 +81,6 @@ namespace obelisk { VerifyParameters(); CreateUnitreePublishers(); - // Initialize logging files if logging is enabled - if (logging_) { - startup_time_ = this->now(); - InitializeLogging(); - } - loco_client_.SetTimeout(command_timeout_); loco_client_.Init(); motion_switcher_client_ = std::make_shared(); @@ -110,9 +101,6 @@ namespace obelisk { // Need to create after the publishers have been activated lowstate_subscriber_.reset(new ChannelSubscriber(STATE_TOPIC_)); lowstate_subscriber_->InitChannel(std::bind(&G1Interface::LowStateHandler, this, std::placeholders::_1), 10); - - main_board_subscriber_.reset(new ChannelSubscriber(MAIN_BOARD_STATE_TOPIC_)); - main_board_subscriber_->InitChannel(std::bind(&G1Interface::MainboardHandler, this, std::placeholders::_1), 10); } // TODO: Adjust this function so that we can go to a user pose even when no low level control functions are being sent @@ -241,9 +229,6 @@ namespace obelisk { joint_state.joint_names.at(i) = G1_FULL_JOINT_NAMES[i]; joint_state.joint_pos.at(i) = low_state.motor_state()[i].q(); joint_state.joint_vel.at(i) = low_state.motor_state()[i].dq(); - // RCLCPP_INFO_STREAM(this->get_logger(), "Joint: " << joint_state.joint_names.at(i) << " motor state: " << low_state.motor_state()[i].motorstate()); - // if (low_state.motor_state()[i].motorstate() && i <= RightAnkleRoll) // TODO: What is this? - // std::cout << "[ERROR] motor " << i << " with code " << low_state.motor_state()[i].motorstate() << "\n"; } this->GetPublisher(pub_joint_state_key_)->publish(joint_state); @@ -267,8 +252,9 @@ namespace obelisk { this->GetPublisher(pub_imu_state_key_)->publish(imu_state); // Log motor data if logging is enabled - if (logging_ && this->log_count_ % 1000 == 0) { // Log at 1 Hz assuming 1kHz low state rate - LogMotorData(low_state); + if (logging_) { + std::lock_guard lk(motor_state_mtx_); + motor_state_ = low_state.motor_state(); } // update mode machine @@ -278,19 +264,6 @@ namespace obelisk { } } - void MainboardHandler(const void *message) { - MainBoardState_ board_state = *(const MainBoardState_ *)message; - - // if (board_state.crc() != Crc32Core((uint32_t *)&board_state, (sizeof(MainBoardState_) >> 2) - 1)) { - // RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeRobotInterface] MainBoardState CRC Error"); - // return; - // } - - RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Main board state: " << board_state.state()[0] << ", " << board_state.state()[1] << ", " << board_state.state()[2] << ", " << board_state.state()[3] << ", " << board_state.state()[4] << ", " << board_state.state()[5] ); - - RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Main board value: " << board_state.value()[0] << ", " << board_state.value()[1] << ", " << board_state.value()[2] << ", " << board_state.value()[3] << ", " << board_state.value()[4] << ", " << board_state.value()[5] ); - } - void ApplyVelCmd(const unitree_vel_cmd_msg& msg) override { if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; @@ -350,11 +323,7 @@ namespace obelisk { return true; } - void InitializeLogging() { - // Create and open motor data log file in the existing log directory - std::string motor_data_file = log_dir_path_ + "/motor_data.csv"; - motor_data_log_.open(motor_data_file); - + void WriteMotorLogHeader() override{ if (motor_data_log_.is_open()) { // Write CSV header with startup time info motor_data_log_ << "# Logging started at ROS time: " << startup_time_.nanoseconds() << " ns\n"; @@ -368,12 +337,12 @@ namespace obelisk { } motor_data_log_ << "\n"; motor_data_log_.flush(); - - RCLCPP_INFO_STREAM(this->get_logger(), "Motor data logging initialized: " << motor_data_file); } else { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open motor data log file: " << motor_data_file); + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed write motor data log file header"); } - + } + + void WriteJointNameFile() override{ // Create joint names file std::string joint_names_file = log_dir_path_ + "/joint_names.txt"; std::ofstream joint_names_log(joint_names_file); @@ -387,8 +356,14 @@ namespace obelisk { } } - void LogMotorData(const LowState_& low_state) { + void WriteMotorData() { if (!motor_data_log_.is_open()) return; + std::array motor_state; + { + std::lock_guard lk(motor_state_mtx_); + motor_state = motor_state_; + } + // Get current time relative to startup in seconds auto current_time = this->now(); @@ -399,7 +374,7 @@ namespace obelisk { // Log data for each motor for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { - const auto& motor = low_state.motor_state()[i]; + const auto& motor = motor_state[i]; motor_data_log_ << "," << motor.temperature()[0] << "," << motor.temperature()[1] << "," << motor.tau_est() @@ -410,14 +385,14 @@ namespace obelisk { this->log_count_++; } + inline static const std::string ROBOT_NAME = "g1"; + const std::string& RobotName() const override { return ROBOT_NAME; } + bool use_hands_; bool fixed_waist_; - std::string MAIN_BOARD_STATE_TOPIC_; - ChannelSubscriberPtr lowstate_subscriber_; unitree::robot::ChannelPublisherPtr lowcmd_publisher_; - ChannelSubscriberPtr main_board_subscriber_; enum class AnkleMode { PR = 0, // Series Control for Ptich/Roll Joints @@ -427,10 +402,6 @@ namespace obelisk { AnkleMode mode_pr_; uint8_t mode_machine_; - // Logging - std::ofstream motor_data_log_; - rclcpp::Time startup_time_; - private: // ---------- Robot Specific ---------- // // G1 diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 5d59fe89..dbb15e65 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -12,10 +12,10 @@ namespace obelisk { using namespace unitree_go::msg::dds_; - class Go2Interface : public ObeliskUnitreeInterface { + class Go2Interface : public ObeliskUnitreeInterface { public: Go2Interface(const std::string& node_name) - : ObeliskUnitreeInterface(node_name) { + : ObeliskUnitreeInterface(node_name, "go2") { // Expose duration of transition to home position as ros parameter this->declare_parameter("user_pose_transition_duration", 1.); @@ -215,6 +215,11 @@ namespace obelisk { imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2]; this->GetPublisher(pub_imu_state_key_)->publish(imu_state); + + if (logging_) { + std::lock_guard lk(motor_state_mtx_); + motor_state_ = low_state.motor_state(); + } } void ApplyVelCmd(const unitree_vel_cmd_msg& msg) override { @@ -269,6 +274,66 @@ namespace obelisk { return ret == 0; } + void WriteMotorLogHeader() override{ + if (motor_data_log_.is_open()) { + // Write CSV header with startup time info + motor_data_log_ << "# Logging started at ROS time: " << startup_time_.nanoseconds() << " ns\n"; + motor_data_log_ << "# Timestamps below are relative to startup in seconds\n"; + motor_data_log_ << "timestamp_sec"; + for (size_t i = 0; i < num_motors_; ++i) { + motor_data_log_ << ",temp_" << joint_names_[i] + << ",tau_est_" << joint_names_[i]; + } + motor_data_log_ << "\n"; + motor_data_log_.flush(); + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed write motor data log file header"); + } + } + + void WriteJointNameFile() override{ + // Create joint names file + std::string joint_names_file = log_dir_path_ + "/joint_names.txt"; + std::ofstream joint_names_log(joint_names_file); + if (joint_names_log.is_open()) { + joint_names_log << "Joint order for logged data:\n"; + for (size_t i = 0; i < num_motors_; ++i) { + joint_names_log << i << ": " << joint_names_[i] << "\n"; + } + joint_names_log.close(); + RCLCPP_INFO_STREAM(this->get_logger(), "Joint names file created: " << joint_names_file); + } + } + + void WriteMotorData() { + if (!motor_data_log_.is_open()) return; + std::array motor_state; + { + std::lock_guard lk(motor_state_mtx_); + motor_state = motor_state_; + } + + // Get current time relative to startup in seconds + auto current_time = this->now(); + auto elapsed_duration = current_time - startup_time_; + double timestamp_sec = elapsed_duration.seconds(); + + motor_data_log_ << std::fixed << std::setprecision(6) << timestamp_sec; + + // Log data for each motor + for (size_t i = 0; i < num_motors_; ++i) { + const auto& motor = motor_state[i]; + motor_data_log_ << "," << motor.temperature() + << "," << motor.tau_est(); + } + motor_data_log_ << "\n"; + + this->log_count_++; + } + + inline static const std::string ROBOT_NAME = "go2"; + const std::string& RobotName() const override { return ROBOT_NAME; } + unitree::robot::ChannelPublisherPtr lowcmd_publisher_; ChannelSubscriberPtr lowstate_subscriber_; diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index ed8f01e7..ca36e81b 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -11,6 +11,7 @@ #include #include #include +#include // DDS #include @@ -28,10 +29,11 @@ namespace obelisk { /** * @brief Obelisk robot interface for the Unitree robots */ + template class ObeliskUnitreeInterface : public ObeliskRobot { public: - ObeliskUnitreeInterface(const std::string& node_name) + ObeliskUnitreeInterface(const std::string& node_name, const std::string& robot_name) : ObeliskRobot(node_name) { // Get network interface name as a parameter @@ -46,18 +48,33 @@ namespace obelisk { this->RegisterObkPublisher("pub_sensor_setting", pub_joint_state_key_); this->RegisterObkPublisher("pub_imu_setting", pub_imu_state_key_); + // Register Execution FSM Subscriber + this->RegisterObkSubscription( + "sub_fsm_setting", sub_fsm_key_, std::bind(&ObeliskUnitreeInterface::TransitionFSM, this, std::placeholders::_1)); + // Register High Level Control Subscriber + this->RegisterObkSubscription( + "sub_vel_cmd_setting", sub_UNITREE_VEL_CTRL_key_, std::bind(&ObeliskUnitreeInterface::ApplyVelCmd, this, std::placeholders::_1)); + + // ---------- Default PD gains ---------- // + this->declare_parameter>("default_kp"); + this->declare_parameter>("default_kd"); + kp_ = this->get_parameter("default_kp").as_double_array(); + kd_ = this->get_parameter("default_kd").as_double_array(); + + // Init the channel factor for hardware coms + ChannelFactory::Instance()->Init(0, network_interface_name_); + // Optional low level logging this->declare_parameter("logging", false); logging_ = this->get_parameter("logging").as_bool(); - // Create logging directory if logging is enabled if (logging_) { auto now = std::chrono::system_clock::now(); auto time_t = std::chrono::system_clock::to_time_t(now); std::stringstream ss; ss << std::put_time(std::localtime(&time_t), "%Y-%m-%d_%H-%M-%S"); - std::filesystem::path unitree_logs_dir = "unitree_logs"; + std::filesystem::path unitree_logs_dir = "unitree_" + robot_name + "_logs"; std::filesystem::path session_dir = unitree_logs_dir / ss.str(); std::filesystem::create_directories(session_dir); @@ -65,28 +82,13 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "Created logging directory: " << session_dir); log_count_ = 0; - } - // Register Execution FSM Subscriber - this->RegisterObkSubscription( - "sub_fsm_setting", sub_fsm_key_, std::bind(&ObeliskUnitreeInterface::TransitionFSM, this, std::placeholders::_1)); - // Register High Level Control Subscriber - this->RegisterObkSubscription( - "sub_vel_cmd_setting", sub_UNITREE_VEL_CTRL_key_, std::bind(&ObeliskUnitreeInterface::ApplyVelCmd, this, std::placeholders::_1)); - - // ---------- Default PD gains ---------- // - this->declare_parameter>("default_kp"); - this->declare_parameter>("default_kd"); - kp_ = this->get_parameter("default_kp").as_double_array(); - kd_ = this->get_parameter("default_kd").as_double_array(); - - // TODO: User defined safety maybe - ChannelFactory::Instance()->Init(0, network_interface_name_); - - // Try to shutdown motion control-related service - // mode_switch_manager_ = std::make_shared(); - // mode_switch_manager_->SetTimeout(5.0f); - // mode_switch_manager_->Init(); + startup_time_ = this->now(); + InitializeLogging(); + // TODO: Tie timer callback to WriteMotorData() + this->RegisterObkTimer("timer_logging_setting", timer_logging_key_, + std::bind(&ObeliskUnitreeInterface::WriteMotorData, this)); + } } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_activate( @@ -139,6 +141,10 @@ namespace obelisk { virtual void TransitionToDamp() = 0; virtual bool EngageUnitreeMotionControl() = 0; virtual bool ReleaseUnitreeMotionControl() = 0; + virtual void WriteMotorData() = 0; + virtual void WriteMotorLogHeader() = 0; + virtual void WriteJointNameFile() = 0; + virtual const std::string& RobotName() const = 0; void TransitionFSM(const unitree_fsm_msg& msg) { // Extract commanded FSM state from the message @@ -249,11 +255,26 @@ namespace obelisk { return std::find(transitions[state].begin(), transitions[state].end(), next_state) != transitions[state].end(); } + void InitializeLogging() { + // Create and open motor data log file in the existing log directory + std::string motor_data_file = log_dir_path_ + "/motor_data.csv"; + motor_data_log_.open(motor_data_file); + + if (motor_data_log_.is_open()) { + WriteMotorLogHeader(); + motor_data_log_.flush(); + RCLCPP_INFO_STREAM(this->get_logger(), "Motor data logging initialized: " << motor_data_file); + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open motor data log file: " << motor_data_file); + } + + WriteJointNameFile(); + } + // Execution FSM state variable ExecFSMState exec_fsm_state_; // ---------- Topics ---------- // - // TODO: Verify these are the same of the G1 and the Go2 std::string CMD_TOPIC_; std::string STATE_TOPIC_; @@ -262,6 +283,12 @@ namespace obelisk { size_t num_motors_; std::vector joint_names_; + // For motor logging + std::ofstream motor_data_log_; + rclcpp::Time startup_time_; + std::array motor_state_; + mutable std::mutex motor_state_mtx_; + // ---------- Gains ---------- // std::vector kp_; std::vector kd_; @@ -271,11 +298,13 @@ namespace obelisk { const std::string sub_UNITREE_VEL_CTRL_key_ = "sub_vel_cmd_key"; const std::string pub_joint_state_key_ = "joint_state_pub"; const std::string pub_imu_state_key_ = "imu_state_pub"; + const std::string timer_logging_key_ = "timer_logging_key"; // Logging bool logging_; std::string log_dir_path_; long log_count_; + private: }; diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index e6fdaf45..698c1387 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -44,7 +44,7 @@ onboard: topic: /obelisk/g1/joy timers: - ros_parameter: timer_ctrl_setting - timer_period_sec: 100 # Control callback is not being used + timer_period_sec: 0.1 callback_group: None estimation: - pkg: obelisk_unitree_cpp From 0770aba09808b8975d566744d03ed14bfcc87a73 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 16:40:10 -0800 Subject: [PATCH 09/27] changed damping to work explicitly in either high or low level control --- .../hardware/robots/unitree/g1_interface.h | 54 ++++++++++++++++--- .../hardware/robots/unitree/go2_interface.h | 26 ++++++++- .../robots/unitree/unitree_interface.h | 7 ++- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 11 ++++ .../src/obelisk_ros/config/go2_cpp.yaml | 6 +++ 5 files changed, 95 insertions(+), 9 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index adf20d47..3e845540 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -292,11 +292,40 @@ namespace obelisk { } void TransitionToDamp() override{ - loco_client_.StopMove(); - loco_client_.Damp(); + if (high_level_ctrl_engaged_) { + // use high level damping + loco_client_.Damp(); + } else { + // use low level damping + LowCmd_ dds_low_command; + for (size_t j = 0; j < G1_27DOF; j++) { // Only go through the non-hand motors + int i = G1_JOINT_MAPPINGS.at(joint_names_[j]); + dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(i).tau() = 0; + dds_low_command.motor_cmd().at(i).q() = 0; + dds_low_command.motor_cmd().at(i).dq() = 0; + dds_low_command.motor_cmd().at(i).kp() = 0; + dds_low_command.motor_cmd().at(i).kd() = kd_damping_[i]; + } + if (fixed_waist_) { + for (size_t j = 0; j < G1_EXTRA_WAIST; j++) { + int i = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[j]); + dds_low_command.motor_cmd().at(i).mode() = 0; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(i).tau() = 0; + dds_low_command.motor_cmd().at(i).q() = 0; + dds_low_command.motor_cmd().at(i).dq() = 0; + dds_low_command.motor_cmd().at(i).kp() = 0; + dds_low_command.motor_cmd().at(i).kd() = 0; + } + } + dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1); + lowcmd_publisher_->Write(dds_low_command); + } + } bool ReleaseUnitreeMotionControl() { + loco_client_.StopMove(); std::string robot_form, motion_mode; int32_t ret = motion_switcher_client_->CheckMode(robot_form, motion_mode); if (ret != 0) { @@ -304,10 +333,17 @@ namespace obelisk { return false; } + bool ret_b; if (!motion_mode.empty()) { - return motion_switcher_client_->ReleaseMode() == 0; + ret_b = motion_switcher_client_->ReleaseMode() == 0; + } else { + ret_b = true; + } + if (ret_b) { + high_level_ctrl_engaged_ = false; } - return true; + + return ret_b; } bool EngageUnitreeMotionControl() { @@ -317,10 +353,16 @@ namespace obelisk { RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); return false; } + bool ret_b; if (motion_mode.empty()) { - return motion_switcher_client_->SelectMode("normal") == 0; + ret_b = motion_switcher_client_->SelectMode("normal") == 0; + } else { + ret_b = true; + } + if (ret_b) { + high_level_ctrl_engaged_ = true; } - return true; + return ret_b; } void WriteMotorLogHeader() override{ diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index dbb15e65..11a86a10 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -258,19 +258,41 @@ namespace obelisk { } void TransitionToDamp() override{ - sport_client_.StopMove(); - sport_client_.Damp(); + if (high_level_ctrl_engaged_) { + // Use high level damping + sport_client_.StopMove(); + sport_client_.Damp(); + } else { + // Use low level damping + LowCmd_ dds_low_command; + for (size_t i = 0; i < num_motors_; i++) { // Only go through the non-hand motors + dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(i).tau() = 0; + dds_low_command.motor_cmd().at(i).q() = 0; + dds_low_command.motor_cmd().at(i).dq() = 0; + dds_low_command.motor_cmd().at(i).kp() = 0; + dds_low_command.motor_cmd().at(i).kd() = kd_damping_[i]; + } + dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1); + lowcmd_publisher_->Write(dds_low_command); + } } bool EngageUnitreeMotionControl() override{ int32_t ret; robot_state_client_.ServiceSwitch("mcf", 1, ret); // Turn the Unitree motion control on + if (ret == 0) { + high_level_ctrl_engaged_ = true; + } return ret == 0; } bool ReleaseUnitreeMotionControl() override{ int32_t ret; robot_state_client_.ServiceSwitch("mcf", 0, ret); // Turn the Unitree motion control off + if (ret == 0) { + high_level_ctrl_engaged_ = false; + } return ret == 0; } diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index ca36e81b..07998cf1 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -43,6 +43,7 @@ namespace obelisk { // Set the Execution FSM into INIT exec_fsm_state_ = ExecFSMState::INIT; + high_level_ctrl_engaged_ = true; // Additional Publishers this->RegisterObkPublisher("pub_sensor_setting", pub_joint_state_key_); @@ -58,8 +59,10 @@ namespace obelisk { // ---------- Default PD gains ---------- // this->declare_parameter>("default_kp"); this->declare_parameter>("default_kd"); + this->declare_parameter>("default_kd_damping"); kp_ = this->get_parameter("default_kp").as_double_array(); kd_ = this->get_parameter("default_kd").as_double_array(); + kd_damping_ = this->get_parameter("default_kd_damping").as_double_array(); // Init the channel factor for hardware coms ChannelFactory::Instance()->Init(0, network_interface_name_); @@ -123,7 +126,7 @@ namespace obelisk { // Set of transitions on which motion control must be released const std::vector RELEASE_MC_STATES = { - ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP + ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::ESTOP }; // Set of transitions on which motion control must be engaged @@ -273,6 +276,7 @@ namespace obelisk { // Execution FSM state variable ExecFSMState exec_fsm_state_; + bool high_level_ctrl_engaged_; // ---------- Topics ---------- // std::string CMD_TOPIC_; @@ -292,6 +296,7 @@ namespace obelisk { // ---------- Gains ---------- // std::vector kp_; std::vector kd_; + std::vector kd_damping_; // Keys const std::string sub_fsm_key_ = "sub_exec_fsm_key"; diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 698c1387..be3d9afe 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -102,6 +102,17 @@ onboard: # 1., 1., 1., # R shoulder # 1., 1., 1., 1., # R elbow, wrist # ] + # default_kd_damping: [ + # 10., 10., 10., # L hip + # 10., 10., 10., # L knee, ankle + # 10., 10., 10., # R hip + # 10., 10., 10., # R knee, ankle + # 10., #10., 10., # Waist + # 10., 10., 10., # L shoulder + # 10., 10., 10., 10., # L elbow, wrist + # 10., 10., 10., # R shoulder + # 10., 10., 10., 10., # R elbow, wrist + # ] # ================== # callback_groups: publishers: diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index 31287381..34031f52 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -98,6 +98,12 @@ onboard: 5., 5., 5., # RR 5., 5., 5., # RL ] + default_kd_damping: [ + 15., 15., 15., # FR + 15., 15., 15., # FL + 15., 15., 15., # RR + 15., 15., 15., # RL + ] # ================== # callback_groups: publishers: From 1d4c8ea1b0cc0d03199e0e00d7608ea661104429 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 16:44:23 -0800 Subject: [PATCH 10/27] add timer config --- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 3 +++ obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml | 3 +++ 2 files changed, 6 insertions(+) diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index be3d9afe..b8510ac3 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -260,6 +260,9 @@ onboard: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None viz: on: True viz_tool: foxglove diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index 34031f52..eaa29ea7 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -196,6 +196,9 @@ onboard: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None viz: on: True viz_tool: foxglove From 86c3b4eb839020d7a54ccd9baec34128a55cf20a Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 17:28:30 -0800 Subject: [PATCH 11/27] humanoid example fixed --- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 39 +- .../src/obelisk_ros/config/go2_cpp.yaml | 54 +- .../robots/g1_description/mujoco/g1_27dof.xml | 930 ++++++++++++++++++ .../mujoco/g1_27dof_fixed_base.xml | 846 ++++++++++++++++ ....xml => g1_29dof_with_hand_fixed_base.xml} | 0 .../{basic_scene.xml => g1_basic_scene.xml} | 2 +- 6 files changed, 1807 insertions(+), 64 deletions(-) create mode 100644 obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml create mode 100644 obelisk_ws/src/robots/g1_description/mujoco/g1_27dof_fixed_base.xml rename obelisk_ws/src/robots/g1_description/mujoco/{g1_fixed_base.xml => g1_29dof_with_hand_fixed_base.xml} (100%) rename obelisk_ws/src/robots/g1_description/mujoco/{basic_scene.xml => g1_basic_scene.xml} (95%) diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index b8510ac3..66f97a8c 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -150,7 +150,9 @@ onboard: time_step: 0.002 num_steps_per_viz: 5 robot_pkg: g1_description - model_xml_path: g1_fixed_base.xml + # model_xml_path: g1_27dof_fixed_base.xml + # model_xml_path: g1_27dof.xml + model_xml_path: g1_basic_scene.xml sensor_settings: - topic: /obelisk/g1/joint_encoders dt: 0.002 @@ -172,8 +174,6 @@ onboard: right_ankle_roll_joint_pos_sensor: jointpos waist_yaw_joint_pos_sensor: jointpos - waist_roll_joint_pos_sensor: jointpos - waist_pitch_joint_pos_sensor: jointpos left_shoulder_pitch_joint_pos_sensor: jointpos left_shoulder_roll_joint_pos_sensor: jointpos @@ -183,14 +183,6 @@ onboard: left_wrist_pitch_joint_pos_sensor: jointpos left_wrist_yaw_joint_pos_sensor: jointpos - left_hand_thumb_0_joint_pos_sensor: jointpos - left_hand_thumb_1_joint_pos_sensor: jointpos - left_hand_thumb_2_joint_pos_sensor: jointpos - left_hand_middle_0_joint_pos_sensor: jointpos - left_hand_middle_1_joint_pos_sensor: jointpos - left_hand_index_0_joint_pos_sensor: jointpos - left_hand_index_1_joint_pos_sensor: jointpos - right_shoulder_pitch_joint_pos_sensor: jointpos right_shoulder_roll_joint_pos_sensor: jointpos right_shoulder_yaw_joint_pos_sensor: jointpos @@ -199,13 +191,6 @@ onboard: right_wrist_pitch_joint_pos_sensor: jointpos right_wrist_yaw_joint_pos_sensor: jointpos - right_hand_thumb_0_joint_pos_sensor: jointpos - right_hand_thumb_1_joint_pos_sensor: jointpos - right_hand_thumb_2_joint_pos_sensor: jointpos - right_hand_middle_0_joint_pos_sensor: jointpos - right_hand_middle_1_joint_pos_sensor: jointpos - right_hand_index_0_joint_pos_sensor: jointpos - right_hand_index_1_joint_pos_sensor: jointpos # ---------- Joint Velocities ---------- # left_hip_pitch_joint_vel_sensor: jointvel left_hip_roll_joint_vel_sensor: jointvel @@ -222,8 +207,6 @@ onboard: right_ankle_roll_joint_vel_sensor: jointvel waist_yaw_joint_vel_sensor: jointvel - waist_roll_joint_vel_sensor: jointvel - waist_pitch_joint_vel_sensor: jointvel left_shoulder_pitch_joint_vel_sensor: jointvel left_shoulder_roll_joint_vel_sensor: jointvel @@ -233,14 +216,6 @@ onboard: left_wrist_pitch_joint_vel_sensor: jointvel left_wrist_yaw_joint_vel_sensor: jointvel - left_hand_thumb_0_joint_vel_sensor: jointvel - left_hand_thumb_1_joint_vel_sensor: jointvel - left_hand_thumb_2_joint_vel_sensor: jointvel - left_hand_middle_0_joint_vel_sensor: jointvel - left_hand_middle_1_joint_vel_sensor: jointvel - left_hand_index_0_joint_vel_sensor: jointvel - left_hand_index_1_joint_vel_sensor: jointvel - right_shoulder_pitch_joint_vel_sensor: jointvel right_shoulder_roll_joint_vel_sensor: jointvel right_shoulder_yaw_joint_vel_sensor: jointvel @@ -248,14 +223,6 @@ onboard: right_wrist_roll_joint_vel_sensor: jointvel right_wrist_pitch_joint_vel_sensor: jointvel right_wrist_yaw_joint_vel_sensor: jointvel - - right_hand_thumb_0_joint_vel_sensor: jointvel - right_hand_thumb_1_joint_vel_sensor: jointvel - right_hand_thumb_2_joint_vel_sensor: jointvel - right_hand_middle_0_joint_vel_sensor: jointvel - right_hand_middle_1_joint_vel_sensor: jointvel - right_hand_index_0_joint_vel_sensor: jointvel - right_hand_index_1_joint_vel_sensor: jointvel timers: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index eaa29ea7..be89fb99 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -75,35 +75,35 @@ onboard: # sensing: robot: # === simulation === - # - is_simulated: True - # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_sim - # params: - # ic_keyframe: standing - # === hardware === - - is_simulated: False + - is_simulated: True pkg: obelisk_unitree_cpp - executable: obelisk_unitree_go2_hardware + executable: obelisk_unitree_sim params: - network_interface_name: enp4s0 #enx001cc24ce09a - default_kp: [ - 60., 60., 60., # FR - 60., 60., 60., # FL - 60., 60., 60., # RR - 60., 60., 60., # RL - ] - default_kd: [ - 5., 5., 5., # FR - 5., 5., 5., # FL - 5., 5., 5., # RR - 5., 5., 5., # RL - ] - default_kd_damping: [ - 15., 15., 15., # FR - 15., 15., 15., # FL - 15., 15., 15., # RR - 15., 15., 15., # RL - ] + ic_keyframe: standing + # === hardware === + # - is_simulated: False + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_go2_hardware + # params: + # network_interface_name: enp4s0 #enx001cc24ce09a + # default_kp: [ + # 60., 60., 60., # FR + # 60., 60., 60., # FL + # 60., 60., 60., # RR + # 60., 60., 60., # RL + # ] + # default_kd: [ + # 5., 5., 5., # FR + # 5., 5., 5., # FL + # 5., 5., 5., # RR + # 5., 5., 5., # RL + # ] + # default_kd_damping: [ + # 15., 15., 15., # FR + # 15., 15., 15., # FL + # 15., 15., 15., # RR + # 15., 15., 15., # RL + # ] # ================== # callback_groups: publishers: diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml new file mode 100644 index 00000000..5f85bc14 --- /dev/null +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml @@ -0,0 +1,930 @@ + + + + + \ No newline at end of file diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof_fixed_base.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof_fixed_base.xml new file mode 100644 index 00000000..7621345f --- /dev/null +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof_fixed_base.xml @@ -0,0 +1,846 @@ + + + + + \ No newline at end of file diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_fixed_base.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_29dof_with_hand_fixed_base.xml similarity index 100% rename from obelisk_ws/src/robots/g1_description/mujoco/g1_fixed_base.xml rename to obelisk_ws/src/robots/g1_description/mujoco/g1_29dof_with_hand_fixed_base.xml diff --git a/obelisk_ws/src/robots/g1_description/mujoco/basic_scene.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml similarity index 95% rename from obelisk_ws/src/robots/g1_description/mujoco/basic_scene.xml rename to obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml index c0f74146..3123f071 100644 --- a/obelisk_ws/src/robots/g1_description/mujoco/basic_scene.xml +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml @@ -1,5 +1,5 @@ - + From 6bda3881193341696086e0fc8aae544c4b9d9914 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 7 Nov 2025 17:45:42 -0800 Subject: [PATCH 12/27] fixed high level command --- .../cpp/hardware/include/unitree_joystick.h | 12 +++-- .../src/obelisk_ros/config/go2_cpp.yaml | 54 +++++++++---------- 2 files changed, 35 insertions(+), 31 deletions(-) diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index e876fe67..a736c3a1 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -209,9 +209,9 @@ namespace obelisk { void UpdateXHat(__attribute__((unused)) const sensor_msgs::msg::Joy& msg) override { vel_cmd_msg vel_msg; vel_msg.header.stamp = this->now(); - vel_msg.v_x = msg.axes[static_cast(ButtonMap::LY)] * v_x_scale_; - vel_msg.v_y = msg.axes[static_cast(ButtonMap::LX)] * v_y_scale_; - vel_msg.w_z = msg.axes[static_cast(ButtonMap::RX)] * w_z_scale_; + vel_msg.v_x = getAxisValue(msg, vel_cmd_x_ - AXIS_OFFSET) * v_x_scale_; + vel_msg.v_y = getAxisValue(msg, vel_cmd_y_ - AXIS_OFFSET) * v_y_scale_; + vel_msg.w_z = getAxisValue(msg, vel_cmd_yaw_ - AXIS_OFFSET) * w_z_scale_; this->GetPublisher(this->ctrl_key_)->publish(vel_msg); @@ -307,7 +307,7 @@ namespace obelisk { btn -= LAYER_OFFSET; } if (on_axis) { - return msg.axes[btn - AXIS_OFFSET] < axis_threshold_; + return getAxisValue(msg, btn - AXIS_OFFSET) < axis_threshold_; } if (on_dpad) { int sgn = 1; @@ -320,6 +320,10 @@ namespace obelisk { return msg.buttons[btn]; } + float getAxisValue(const sensor_msgs::msg::Joy& msg, int ax) { + return msg.axes[ax]; + } + bool isOnLayer(int btn) { return btn == static_cast(ButtonMap::X1) || btn == static_cast(ButtonMap::Y1) || btn == static_cast(ButtonMap::A1) || btn == static_cast(ButtonMap::B1) || btn == static_cast(ButtonMap::DL1) || btn == static_cast(ButtonMap::DR1) || btn == static_cast(ButtonMap::DU1) || btn == static_cast(ButtonMap::DD1); } diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index be89fb99..eaa29ea7 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -75,35 +75,35 @@ onboard: # sensing: robot: # === simulation === - - is_simulated: True - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_sim - params: - ic_keyframe: standing - # === hardware === - # - is_simulated: False + # - is_simulated: True # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_go2_hardware + # executable: obelisk_unitree_sim # params: - # network_interface_name: enp4s0 #enx001cc24ce09a - # default_kp: [ - # 60., 60., 60., # FR - # 60., 60., 60., # FL - # 60., 60., 60., # RR - # 60., 60., 60., # RL - # ] - # default_kd: [ - # 5., 5., 5., # FR - # 5., 5., 5., # FL - # 5., 5., 5., # RR - # 5., 5., 5., # RL - # ] - # default_kd_damping: [ - # 15., 15., 15., # FR - # 15., 15., 15., # FL - # 15., 15., 15., # RR - # 15., 15., 15., # RL - # ] + # ic_keyframe: standing + # === hardware === + - is_simulated: False + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_go2_hardware + params: + network_interface_name: enp4s0 #enx001cc24ce09a + default_kp: [ + 60., 60., 60., # FR + 60., 60., 60., # FL + 60., 60., 60., # RR + 60., 60., 60., # RL + ] + default_kd: [ + 5., 5., 5., # FR + 5., 5., 5., # FL + 5., 5., 5., # RR + 5., 5., 5., # RL + ] + default_kd_damping: [ + 15., 15., 15., # FR + 15., 15., 15., # FL + 15., 15., 15., # RR + 15., 15., 15., # RL + ] # ================== # callback_groups: publishers: From 7f57f29929e5601549668ce22a775dd7837da540 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 8 Nov 2025 11:26:42 -0800 Subject: [PATCH 13/27] joystick update --- obelisk/cpp/hardware/include/unitree_joystick.h | 1 - 1 file changed, 1 deletion(-) diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index a736c3a1..d71157c9 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -269,7 +269,6 @@ namespace obelisk { this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command."); } - } bool recognizeButton(int btn) { From c3563163a6e1a1fe36b4bbd47a9d561edb3582c8 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 8 Nov 2025 15:17:41 -0800 Subject: [PATCH 14/27] fsm functional on go2 --- .../include/unitree_example_controller.h | 82 +++++++++++++++---- obelisk/cpp/hardware/include/unitree_fsm.h | 11 +++ .../cpp/hardware/include/unitree_joystick.h | 2 +- .../hardware/robots/unitree/g1_interface.h | 6 +- .../hardware/robots/unitree/go2_interface.h | 40 +++++++-- .../robots/unitree/unitree_interface.h | 57 +++++++------ .../src/obelisk_ros/config/go2_cpp.yaml | 12 ++- 7 files changed, 154 insertions(+), 56 deletions(-) diff --git a/obelisk/cpp/hardware/include/unitree_example_controller.h b/obelisk/cpp/hardware/include/unitree_example_controller.h index 99cfa176..861bf47d 100644 --- a/obelisk/cpp/hardware/include/unitree_example_controller.h +++ b/obelisk/cpp/hardware/include/unitree_example_controller.h @@ -1,9 +1,11 @@ +#include #include "sensor_msgs/msg/joy.hpp" #include "rclcpp/rclcpp.hpp" #include "obelisk_controller.h" #include "obelisk_ros_utils.h" + namespace obelisk { using unitree_controller_msg = obelisk_control_msgs::msg::PDFeedForward; using unitree_estimator_msg = obelisk_estimator_msgs::msg::EstimatedState; @@ -14,16 +16,18 @@ namespace obelisk { : ObeliskController(name), joint_idx_(0) { this->declare_parameter("robot_str", ""); - std::string robot_str = this->get_parameter("robot_str").as_string(); + robot_str_ = this->get_parameter("robot_str").as_string(); - if (robot_str == "G1") { + if (robot_str_ == "G1") { motor_num_ = G1_MOTOR_NUM; joint_names_ = G1_JOINT_NAMES; - } else if (robot_str == "Go2") { + joint_defaults_ = G1_JOINT_DEFAULTS; + } else if (robot_str_ == "Go2") { motor_num_ = GO2_MOTOR_NUM; joint_names_ = GO2_JOINT_NAMES; + joint_defaults_ = GO2_JOINT_DEFAULTS; } else { - throw std::runtime_error("[UnitreeExampleController] robot_str is invalid!"); + throw std::runtime_error("[UnitreeExampleController] robot_str is invalid! " + std::string(robot_str_) + " -- Must be G1 or Go2"); } // ----- Joystick Subscriber ----- // this->RegisterObkSubscription( @@ -46,23 +50,16 @@ namespace obelisk { msg.kd.clear(); double time_sec = this->get_clock()->now().seconds(); - // TODO With hands we have 43 - float offset = 0; - for (int i = 0; i < motor_num_; i++) { + float default_joint = joint_defaults_.at(joint_names_[i]); if (i == joint_idx_) { - if (joint_names_[i].find("calf") != std::string::npos) { - offset = -1.5; - } else { - offset = 0; - } - msg.u_mujoco.emplace_back(amplitude_ * sin(time_sec) + offset); - msg.pos_target.emplace_back(amplitude_ * sin(time_sec) + offset); + msg.u_mujoco.emplace_back(amplitude_ * sin(time_sec) + default_joint); + msg.pos_target.emplace_back(amplitude_ * sin(time_sec) + default_joint); msg.kp.push_back(20); msg.kd.push_back(2); } else { - msg.u_mujoco.emplace_back(0); - msg.pos_target.emplace_back(0); + msg.u_mujoco.emplace_back(default_joint); + msg.pos_target.emplace_back(default_joint); msg.kp.push_back(10); msg.kd.push_back(1); } @@ -101,8 +98,8 @@ namespace obelisk { // ----- Buttons ----- // // constexpr int A = 0; // constexpr int B = 1; - constexpr int X = 2; - // constexpr int Y = 3; + constexpr int X = 3; + // constexpr int Y = 2; // constexpr int LEFT_BUMPER = 4; // constexpr int RIGHT_BUMPER = 5; @@ -132,6 +129,8 @@ namespace obelisk { int joint_idx_; int motor_num_; std::vector joint_names_; + std::map joint_defaults_; + std::string robot_str_; static constexpr int G1_MOTOR_NUM = 27; const std::vector G1_JOINT_NAMES = { @@ -182,5 +181,52 @@ namespace obelisk { "RL_thigh_joint", "RL_calf_joint" }; + + const std::map GO2_JOINT_DEFAULTS = { + {"FR_hip_joint", 0.}, + {"FR_thigh_joint", 0.9}, + {"FR_calf_joint", -1.8}, + {"FL_hip_joint", 0.}, + {"FL_thigh_joint", 0.9}, + {"FL_calf_joint", -1.8}, + {"RR_hip_joint", 0.}, + {"RR_thigh_joint", 0.9}, + {"RR_calf_joint", -1.8}, + {"RL_hip_joint", 0.}, + {"RL_thigh_joint", 0.9}, + {"RL_calf_joint", -1.8}, + }; + + const std::map G1_JOINT_DEFAULTS = { + {"left_hip_pitch_joint", -0.42}, + {"left_hip_roll_joint", 0.}, + {"left_hip_yaw_joint", 0.}, + {"left_knee_joint", 0.81}, + {"left_ankle_pitch_joint", -0.4}, + {"left_ankle_roll_joint", 0.}, + {"right_hip_pitch_joint", -0.42}, + {"right_hip_roll_joint", 0.}, + {"right_hip_yaw_joint", 0.}, + {"right_knee_joint", 0.81}, + {"right_ankle_pitch_joint", -0.4}, + {"right_ankle_roll_joint", 0.}, + {"waist_yaw_joint", 0.0}, + // {"waist_roll_joint", 0.0}, + // {"waist_pitch_joint", 0.0}, + {"left_shoulder_pitch_joint", 0.0}, + {"left_shoulder_roll_joint", 0.27}, + {"left_shoulder_yaw_joint", 0.0}, + {"left_elbow_joint", 0.5}, + {"left_wrist_roll_joint", 0.0}, + {"left_wrist_pitch_joint", 0.0}, + {"left_wrist_yaw_joint", 0.0}, + {"right_shoulder_pitch_joint", 0.0}, + {"right_shoulder_roll_joint", -0.27}, + {"right_shoulder_yaw_joint", 0.0}, + {"right_elbow_joint", 0.5}, + {"right_wrist_roll_joint", 0.0}, + {"right_wrist_pitch_joint", 0.0}, + {"right_wrist_yaw_joint", 0.0}, + }; }; } // namespace obelisk diff --git a/obelisk/cpp/hardware/include/unitree_fsm.h b/obelisk/cpp/hardware/include/unitree_fsm.h index a92e2875..f6bd1947 100644 --- a/obelisk/cpp/hardware/include/unitree_fsm.h +++ b/obelisk/cpp/hardware/include/unitree_fsm.h @@ -10,4 +10,15 @@ enum class ExecFSMState : uint8_t { UNITREE_VEL_CTRL = 4, DAMPING = 5, ESTOP = 6 +}; + + +const std::unordered_map TRANSITION_STRINGS = { + {ExecFSMState::INIT, "INIT"}, + {ExecFSMState::UNITREE_HOME, "UNITREE_HOME"}, + {ExecFSMState::USER_POSE, "USER_POSE"}, + {ExecFSMState::USER_CTRL, "LOW_LEVEL_CONTROL"}, + {ExecFSMState::UNITREE_VEL_CTRL, "HIGH_LEVEL_CONTROL"}, + {ExecFSMState::DAMPING, "DAMPING"}, + {ExecFSMState::ESTOP, "ESTOP"} }; \ No newline at end of file diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index d71157c9..4c14ca7d 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -267,7 +267,7 @@ namespace obelisk { } if (commanded){ this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); - RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command."); + RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command " << TRANSITION_STRINGS.at(static_cast(fsm_msg.cmd_exec_fsm_state))); } } diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 3e845540..a2fb2937 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -324,6 +324,10 @@ namespace obelisk { } + void TransitionToUnitreeVel() override{ + return; + } + bool ReleaseUnitreeMotionControl() { loco_client_.StopMove(); std::string robot_form, motion_mode; @@ -342,7 +346,6 @@ namespace obelisk { if (ret_b) { high_level_ctrl_engaged_ = false; } - return ret_b; } @@ -361,6 +364,7 @@ namespace obelisk { } if (ret_b) { high_level_ctrl_engaged_ = true; + loco_client_.StopMove(); } return ret_b; } diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 11a86a10..28aea02e 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -227,7 +227,12 @@ namespace obelisk { if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; } - sport_client_.Move(msg.v_x, msg.v_y, msg.w_z); // Command velocity + // RCLCPP_INFO_STREAM(this->get_logger(), "Sending Move Command: " << msg.v_x << ", " << msg.v_y << ", " << msg.w_z); + int32_t ret; + ret = sport_client_.Move(msg.v_x, msg.v_y, msg.w_z); // Command velocity + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Move Command Failed: ret = " << ret); + } } bool CheckDampingToUnitreeHomeTransition() { @@ -254,7 +259,18 @@ namespace obelisk { } void TransitionToUnitreeHome() override{ - sport_client_.BalanceStand(); + // TODO: mutex these so that we don't call 'move' after 'stopmove' here if we are publishing messages? + sport_client_.StopMove(); + int32_t ret; + ret = sport_client_.StandUp(); + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Transition to Unitree Home (stand up) Failed: ret = " << ret); + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + ret = sport_client_.BalanceStand(); + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Transition to Unitree Home (balanced stand) Failed: ret = " << ret); + } } void TransitionToDamp() override{ @@ -278,25 +294,35 @@ namespace obelisk { } } + void TransitionToUnitreeVel() override{ + sport_client_.ClassicWalk(false); + } + bool EngageUnitreeMotionControl() override{ - int32_t ret; - robot_state_client_.ServiceSwitch("mcf", 1, ret); // Turn the Unitree motion control on + int32_t ret, status; + ret = robot_state_client_.ServiceSwitch("mcf", 1, status); // Turn the Unitree motion control on if (ret == 0) { high_level_ctrl_engaged_ = true; + sport_client_.StopMove(); + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "ENGAGING UNITREE MOTION CONTROL FAILED! ret = " << ret); } return ret == 0; } bool ReleaseUnitreeMotionControl() override{ - int32_t ret; - robot_state_client_.ServiceSwitch("mcf", 0, ret); // Turn the Unitree motion control off + sport_client_.StopMove(); + int32_t ret, status; + ret = robot_state_client_.ServiceSwitch("mcf", 0, status); // Turn the Unitree motion control off if (ret == 0) { high_level_ctrl_engaged_ = false; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "RELEASING UNITREE MOTION CONTROL FAILED! ret = " << ret); } return ret == 0; } - void WriteMotorLogHeader() override{ + void WriteMotorLogHeader() override{ if (motor_data_log_.is_open()) { // Write CSV header with startup time info motor_data_log_ << "# Logging started at ROS time: " << startup_time_.nanoseconds() << " ns\n"; diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 07998cf1..0b98c5c6 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -65,7 +65,12 @@ namespace obelisk { kd_damping_ = this->get_parameter("default_kd_damping").as_double_array(); // Init the channel factor for hardware coms - ChannelFactory::Instance()->Init(0, network_interface_name_); + try { + ChannelFactory::Instance()->Init(0, network_interface_name_); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to connect to network interface: " << network_interface_name_); + throw; + } // Optional low level logging this->declare_parameter("logging", false); @@ -103,22 +108,21 @@ namespace obelisk { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - const std::unordered_map TRANSITION_STRINGS = { - {ExecFSMState::INIT, "INIT"}, - {ExecFSMState::UNITREE_HOME, "UNITREE_HOME"}, - {ExecFSMState::USER_POSE, "USER_POSE"}, - {ExecFSMState::USER_CTRL, "LOW_LEVEL_CONTROL"}, - {ExecFSMState::UNITREE_VEL_CTRL, "HIGH_LEVEL_CONTROL"}, - {ExecFSMState::DAMPING, "DAMPING"}, - {ExecFSMState::ESTOP, "ESTOP"} - }; - // Set of legal transitions for the execution FSM + // const std::unordered_map> TRANSITIONS = { + // {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Init -> Home, Init -> Damp + // {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::UNITREE_VEL_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp + // {ExecFSMState::USER_POSE, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Pose -> Home, Pose -> Low, Pose -> Damp + // {ExecFSMState::USER_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Low -> Home, Low -> Pose, Low -> Damp + // {ExecFSMState::UNITREE_VEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // High -> Home, High -> Damp + // {ExecFSMState::DAMPING, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::ESTOP}}, // Damp -> Home, Damp -> Pose + // {ExecFSMState::ESTOP, {}} // Estop (None) + // }; const std::unordered_map> TRANSITIONS = { {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Init -> Home, Init -> Damp {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::UNITREE_VEL_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp - {ExecFSMState::USER_POSE, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Pose -> Home, Pose -> Low, Pose -> Damp - {ExecFSMState::USER_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Low -> Home, Low -> Pose, Low -> Damp + {ExecFSMState::USER_POSE, {ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Pose -> Home, Pose -> Low, Pose -> Damp + {ExecFSMState::USER_CTRL, {ExecFSMState::USER_POSE, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Low -> Home, Low -> Pose, Low -> Damp {ExecFSMState::UNITREE_VEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // High -> Home, High -> Damp {ExecFSMState::DAMPING, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::ESTOP}}, // Damp -> Home, Damp -> Pose {ExecFSMState::ESTOP, {}} // Estop (None) @@ -126,7 +130,7 @@ namespace obelisk { // Set of transitions on which motion control must be released const std::vector RELEASE_MC_STATES = { - ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::ESTOP + ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL }; // Set of transitions on which motion control must be engaged @@ -142,6 +146,7 @@ namespace obelisk { virtual void TransitionToUnitreeHome() = 0; virtual void TransitionToUserPose() = 0; virtual void TransitionToDamp() = 0; + virtual void TransitionToUnitreeVel() = 0; virtual bool EngageUnitreeMotionControl() = 0; virtual bool ReleaseUnitreeMotionControl() = 0; virtual void WriteMotorData() = 0; @@ -152,6 +157,12 @@ namespace obelisk { void TransitionFSM(const unitree_fsm_msg& msg) { // Extract commanded FSM state from the message ExecFSMState cmd_exec_fsm_state = static_cast(msg.cmd_exec_fsm_state); + + if (exec_fsm_state_ == ExecFSMState::ESTOP) { + RCLCPP_ERROR_STREAM(this->get_logger(), "ESTOP TRIGGERED"); + TransitionToDamp(); + throw std::runtime_error("ESTOP TRIGGERED"); + } // Check if transition is legal if (ContainsTransition(TRANSITIONS, exec_fsm_state_, cmd_exec_fsm_state)) { @@ -162,37 +173,33 @@ namespace obelisk { } // Under some transitions, need to release the motion control - if (std::find(RELEASE_MC_STATES.begin(), RELEASE_MC_STATES.end(), cmd_exec_fsm_state) != RELEASE_MC_STATES.end()) { + if (std::find(RELEASE_MC_STATES.begin(), RELEASE_MC_STATES.end(), cmd_exec_fsm_state) != RELEASE_MC_STATES.end()) { // && high_level_ctrl_engaged_ bool success = ReleaseUnitreeMotionControl(); if (!success) { - RCLCPP_ERROR_STREAM(this->get_logger(), "RELEASING UNITREE MOTION CONTROL FAILED!"); + RCLCPP_ERROR_STREAM(this->get_logger(), "FSM TRANSITION " << TRANSITION_STRINGS.at(exec_fsm_state_) << " TO " << TRANSITION_STRINGS.at(cmd_exec_fsm_state) << " FAILED (motion control)"); return; } - RCLCPP_INFO_STREAM(this->get_logger(), "RELEASED UNITREE MOTION CONTROL!"); } // Under some transitions, need to re-engage the motion control - if (std::find(ENGAGE_MC_STATES.begin(), ENGAGE_MC_STATES.end(), cmd_exec_fsm_state) != ENGAGE_MC_STATES.end()) { + if (std::find(ENGAGE_MC_STATES.begin(), ENGAGE_MC_STATES.end(), cmd_exec_fsm_state) != ENGAGE_MC_STATES.end()) { // && (!high_level_ctrl_engaged_) bool success = EngageUnitreeMotionControl(); if (!success) { - RCLCPP_ERROR_STREAM(this->get_logger(), "ENGAGING UNITREE MOTION CONTROL FAILED!"); + RCLCPP_ERROR_STREAM(this->get_logger(), "FSM TRANSITION " << TRANSITION_STRINGS.at(exec_fsm_state_) << " TO " << TRANSITION_STRINGS.at(cmd_exec_fsm_state) << " FAILED (motion control)"); return; } - RCLCPP_INFO_STREAM(this->get_logger(), "ENGAGED UNITREE MOTION CONTROL!"); } // Transition the FSM exec_fsm_state_ = cmd_exec_fsm_state; RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM STATE TRANSITIONED TO " << TRANSITION_STRINGS.at(exec_fsm_state_)); - if (exec_fsm_state_ == ExecFSMState::ESTOP) { - RCLCPP_ERROR_STREAM(this->get_logger(), "ESTOP TRIGGERED"); - TransitionToDamp(); - throw std::runtime_error("ESTOP TRIGGERED"); - } else if (exec_fsm_state_ == ExecFSMState::DAMPING) { + if (exec_fsm_state_ == ExecFSMState::DAMPING) { TransitionToDamp(); } else if (exec_fsm_state_ == ExecFSMState::UNITREE_HOME) { TransitionToUnitreeHome(); + } else if (exec_fsm_state_ == ExecFSMState::UNITREE_VEL_CTRL) { + TransitionToUnitreeVel(); } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { TransitionToUserPose(); } diff --git a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml index eaa29ea7..bbe1681f 100644 --- a/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/go2_cpp.yaml @@ -27,6 +27,10 @@ onboard: # ----- High Level/Execution FSM Controller ----- # - pkg: obelisk_unitree_cpp executable: obelisk_unitree_joystick + # params: + # v_x_scale: 1.0 + # v_y_scale: 0.5 + # w_z_scale: 1.0 # callback_groups: publishers: # ----- Execution FSM ----- # @@ -36,7 +40,7 @@ onboard: callback_group: None # ----- High Level Control ----- # - ros_parameter: pub_ctrl_setting - topic: /obelisk/go2/high_level_ctrl + topic: /obelisk/go2/vel_cmd history_depth: 10 callback_group: None subscribers: @@ -85,7 +89,7 @@ onboard: pkg: obelisk_unitree_cpp executable: obelisk_unitree_go2_hardware params: - network_interface_name: enp4s0 #enx001cc24ce09a + network_interface_name: enx6c1ff724e92e default_kp: [ 60., 60., 60., # FR 60., 60., 60., # FL @@ -135,8 +139,8 @@ onboard: history_depth: 10 callback_group: None # ----- High Level Control ----- # - - ros_parameter: sub_high_level_ctrl_setting - topic: /obelisk/go2/high_level_ctrl + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/go2/vel_cmd history_depth: 10 callback_group: None sim: From 7ca18229076ef455b14a647766a125a29be3483f Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 8 Nov 2025 15:26:29 -0800 Subject: [PATCH 15/27] faster transition timing --- obelisk/cpp/hardware/robots/unitree/go2_interface.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 28aea02e..d49f6d91 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -299,6 +299,9 @@ namespace obelisk { } bool EngageUnitreeMotionControl() override{ + if (high_level_ctrl_engaged_) { + return true; + } int32_t ret, status; ret = robot_state_client_.ServiceSwitch("mcf", 1, status); // Turn the Unitree motion control on if (ret == 0) { @@ -311,6 +314,9 @@ namespace obelisk { } bool ReleaseUnitreeMotionControl() override{ + if (!high_level_ctrl_engaged_) { + return true; + } sport_client_.StopMove(); int32_t ret, status; ret = robot_state_client_.ServiceSwitch("mcf", 0, status); // Turn the Unitree motion control off From 1582c9bdedd9342bca6589326451466972dbf57a Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 8 Nov 2025 19:36:30 -0800 Subject: [PATCH 16/27] g1 fsm functional --- .../hardware/robots/unitree/g1_interface.h | 65 ++++++++++---- .../hardware/robots/unitree/go2_interface.h | 10 +-- .../robots/unitree/unitree_interface.h | 15 ++-- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 90 +++++++++---------- 4 files changed, 104 insertions(+), 76 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index a2fb2937..346ec760 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -24,6 +24,10 @@ namespace obelisk { G1Interface(const std::string& node_name) : ObeliskUnitreeInterface(node_name, "g1"), use_hands_(false), fixed_waist_(true), mode_pr_(AnkleMode::PR), mode_machine_(0) { + // Expose duration of transition to home position as ros parameter + this->declare_parameter("user_pose_transition_duration", 1.); + user_pose_transition_duration_ = this->get_parameter("user_pose_transition_duration").as_double(); + this->declare_parameter("use_hands", false); use_hands_ = this->get_parameter("use_hands").as_bool(); @@ -268,12 +272,16 @@ namespace obelisk { if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; } - loco_client_.Move(msg.v_x, msg.v_y, msg.w_z); + int32_t ret; + ret = loco_client_.Move(msg.v_x, msg.v_y, msg.w_z); + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Move Command Failed: ret = " << ret); + } } bool CheckDampingToUnitreeHomeTransition() { - // TODO: check if system can safely transition to home position - return false; + // Unitree checks this + return true; } void TransitionToUserPose() override{ @@ -288,12 +296,30 @@ namespace obelisk { void TransitionToUnitreeHome() override{ loco_client_.StopMove(); - loco_client_.BalanceStand(); + int32_t ret; + if (exec_fsm_state_ == ExecFSMState::DAMPING) { + ret = loco_client_.StandUp(); + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Transition to Unitree Home (stand up) Failed: ret = " << ret); + return; + } + for (int i = 0; i < 10; i++) { + RCLCPP_INFO_STREAM(this->get_logger(), "Transitioning Robot to stand in " << i << "/" << 10 << " sec"); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + loco_client_.Start(); + } + + ret = loco_client_.BalanceStand(); + if (ret != 0) { + RCLCPP_INFO_STREAM(this->get_logger(), "Transition to Unitree Home (balanced stand) Failed: ret = " << ret); + } } void TransitionToDamp() override{ if (high_level_ctrl_engaged_) { // use high level damping + loco_client_.StopMove(); loco_client_.Damp(); } else { // use low level damping @@ -325,46 +351,55 @@ namespace obelisk { } void TransitionToUnitreeVel() override{ + // TODO: does this need to be placed in a certain mode? return; } - bool ReleaseUnitreeMotionControl() { - loco_client_.StopMove(); + bool EngageUnitreeMotionControl() { + if (high_level_ctrl_engaged_) { + return true; + } std::string robot_form, motion_mode; int32_t ret = motion_switcher_client_->CheckMode(robot_form, motion_mode); if (ret != 0) { RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); return false; } - bool ret_b; - if (!motion_mode.empty()) { - ret_b = motion_switcher_client_->ReleaseMode() == 0; + if (motion_mode.empty()) { + ret_b = motion_switcher_client_->SelectMode("normal") == 0; } else { ret_b = true; } if (ret_b) { - high_level_ctrl_engaged_ = false; + high_level_ctrl_engaged_ = true; + loco_client_.StopMove(); + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "ENGAGING UNITREE MOTION CONTROL FAILED! ret = " << ret); } return ret_b; } - bool EngageUnitreeMotionControl() { + bool ReleaseUnitreeMotionControl() { + if (!high_level_ctrl_engaged_) { + return true; + } + loco_client_.StopMove(); std::string robot_form, motion_mode; int32_t ret = motion_switcher_client_->CheckMode(robot_form, motion_mode); if (ret != 0) { RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Check mode failed. Error code: " << ret ); return false; } + bool ret_b; - if (motion_mode.empty()) { - ret_b = motion_switcher_client_->SelectMode("normal") == 0; + if (!motion_mode.empty()) { + ret_b = motion_switcher_client_->ReleaseMode() == 0; } else { ret_b = true; } if (ret_b) { - high_level_ctrl_engaged_ = true; - loco_client_.StopMove(); + high_level_ctrl_engaged_ = false; } return ret_b; } diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index d49f6d91..40b6a515 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -21,13 +21,9 @@ namespace obelisk { this->declare_parameter("user_pose_transition_duration", 1.); user_pose_transition_duration_ = this->get_parameter("user_pose_transition_duration").as_double(); - // Expose high level velocity deadzone as a ros parameter - this->declare_parameter("velocity_deadzone", 0.01); - vel_deadzone_ = this->get_parameter("velocity_deadzone").as_double(); - // Expose home position as a ros parameter - this->declare_parameter>("home_position", default_user_pose_); - auto user_pose_vector = this->get_parameter("home_position").as_double_array(); // Get as vector + this->declare_parameter>("user_pose", default_user_pose_); + auto user_pose_vector = this->get_parameter("user_pose").as_double_array(); // Get as vector std::copy(user_pose_vector.begin(), user_pose_vector.end(), user_pose_); // Expose command timeout as a ros parameter @@ -227,7 +223,6 @@ namespace obelisk { if (exec_fsm_state_ != ExecFSMState::UNITREE_VEL_CTRL) { return; } - // RCLCPP_INFO_STREAM(this->get_logger(), "Sending Move Command: " << msg.v_x << ", " << msg.v_y << ", " << msg.w_z); int32_t ret; ret = sport_client_.Move(msg.v_x, msg.v_y, msg.w_z); // Command velocity if (ret != 0) { @@ -398,7 +393,6 @@ namespace obelisk { float joint_vel_[GO2_MOTOR_NUM]; // Local copy of joint velocities float start_user_pose_[GO2_MOTOR_NUM]; // For transitioning to home position float user_pose_transition_duration_; // Duration of the transition to home position - float vel_deadzone_; // Deadzone for high level velocity commands float command_timeout_; // How long to wait before timing out unitree commands. std::vector default_user_pose_ = {0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8}; // Default home position diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 0b98c5c6..f8037722 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -189,20 +189,19 @@ namespace obelisk { return; } } - - // Transition the FSM - exec_fsm_state_ = cmd_exec_fsm_state; - RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM STATE TRANSITIONED TO " << TRANSITION_STRINGS.at(exec_fsm_state_)); - if (exec_fsm_state_ == ExecFSMState::DAMPING) { + if (cmd_exec_fsm_state == ExecFSMState::DAMPING) { TransitionToDamp(); - } else if (exec_fsm_state_ == ExecFSMState::UNITREE_HOME) { + } else if (cmd_exec_fsm_state == ExecFSMState::UNITREE_HOME) { TransitionToUnitreeHome(); - } else if (exec_fsm_state_ == ExecFSMState::UNITREE_VEL_CTRL) { + } else if (cmd_exec_fsm_state == ExecFSMState::UNITREE_VEL_CTRL) { TransitionToUnitreeVel(); - } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { + } else if (cmd_exec_fsm_state == ExecFSMState::USER_POSE) { TransitionToUserPose(); } + // Transition the FSM + exec_fsm_state_ = cmd_exec_fsm_state; + RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM STATE TRANSITIONED TO " << TRANSITION_STRINGS.at(exec_fsm_state_)); } else { if (exec_fsm_state_ == cmd_exec_fsm_state) { RCLCPP_INFO_STREAM(this->get_logger(), "EXECUTION FSM AREADY IN COMMANDED STATE " << TRANSITION_STRINGS.at(exec_fsm_state_)); diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 66f97a8c..36d9afff 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -35,7 +35,7 @@ onboard: callback_group: None # ----- High Level Control ----- # - ros_parameter: pub_ctrl_setting - topic: /obelisk/g1/high_level_ctrl + topic: /obelisk/g1/vel_cmd history_depth: 10 callback_group: None subscribers: @@ -69,50 +69,50 @@ onboard: # sensing: robot: # === simulation === - - is_simulated: True - pkg: obelisk_unitree_cpp - executable: obelisk_unitree_sim - params: - ic_keyframe: standing - # === hardware === - # - is_simulated: False + # - is_simulated: True # pkg: obelisk_unitree_cpp - # executable: obelisk_unitree_g1_hardware + # executable: obelisk_unitree_sim # params: - # network_interface_name: enp4s0 - # default_kp: [ - # 10., 10., 10., # L hip - # 10., 10., 10., # L knee, ankle - # 10., 10., 10., # R hip - # 10., 10., 10., # R knee, ankle - # 10., #10., 10., # Waist - # 5., 5., 5., # L shoulder - # 5., 5., 5., 5., # L elbow, wrist - # 5., 5., 5., # R shoulder - # 5., 5., 5., 5., # R elbow, wrist - # ] - # default_kd: [ - # 1., 1., 1., # L hip - # 1., 1., 1., # L knee, ankle - # 1., 1., 1., # R hip - # 1., 1., 1., # R knee, ankle - # 1., #1., 1., # Waist - # 1., 1., 1., # L shoulder - # 1., 1., 1., 1., # L elbow, wrist - # 1., 1., 1., # R shoulder - # 1., 1., 1., 1., # R elbow, wrist - # ] - # default_kd_damping: [ - # 10., 10., 10., # L hip - # 10., 10., 10., # L knee, ankle - # 10., 10., 10., # R hip - # 10., 10., 10., # R knee, ankle - # 10., #10., 10., # Waist - # 10., 10., 10., # L shoulder - # 10., 10., 10., 10., # L elbow, wrist - # 10., 10., 10., # R shoulder - # 10., 10., 10., 10., # R elbow, wrist - # ] + # ic_keyframe: standing + # === hardware === + - is_simulated: False + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_g1_hardware + params: + network_interface_name: enx6c1ff724e92e + default_kp: [ + 10., 10., 10., # L hip + 10., 10., 10., # L knee, ankle + 10., 10., 10., # R hip + 10., 10., 10., # R knee, ankle + 10., #10., 10., # Waist + 5., 5., 5., # L shoulder + 5., 5., 5., 5., # L elbow, wrist + 5., 5., 5., # R shoulder + 5., 5., 5., 5., # R elbow, wrist + ] + default_kd: [ + 1., 1., 1., # L hip + 1., 1., 1., # L knee, ankle + 1., 1., 1., # R hip + 1., 1., 1., # R knee, ankle + 1., #1., 1., # Waist + 1., 1., 1., # L shoulder + 1., 1., 1., 1., # L elbow, wrist + 1., 1., 1., # R shoulder + 1., 1., 1., 1., # R elbow, wrist + ] + default_kd_damping: [ + 10., 10., 10., # L hip + 10., 10., 10., # L knee, ankle + 10., 10., 10., # R hip + 10., 10., 10., # R knee, ankle + 10., #10., 10., # Waist + 10., 10., 10., # L shoulder + 10., 10., 10., 10., # L elbow, wrist + 10., 10., 10., # R shoulder + 10., 10., 10., 10., # R elbow, wrist + ] # ================== # callback_groups: publishers: @@ -140,8 +140,8 @@ onboard: history_depth: 10 callback_group: None # ----- High Level Control ----- # - - ros_parameter: sub_high_level_ctrl_setting - topic: /obelisk/g1/high_level_ctrl + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/g1/vel_cmd history_depth: 10 callback_group: None sim: From 34256a3428d6089a7561a431ab9a26e4c927a105 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 09:15:10 -0800 Subject: [PATCH 17/27] joystick passthrough and enter low level control by default --- .../cpp/hardware/include/unitree_joystick.h | 140 ++++++++++++++---- .../robots/unitree/unitree_interface.h | 7 +- .../cpp/obelisk_cpp/include/obelisk_node.h | 2 +- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 2 +- .../obelisk_ros/config/unitree_fsm_debug.yaml | 4 + 5 files changed, 125 insertions(+), 30 deletions(-) diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index 4c14ca7d..8f95fc51 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -64,7 +64,15 @@ namespace obelisk { axis_threshold_ = this->get_parameter("axis_threshold").as_double(); // Handle Execution FSM buttons - // Buttons with negative values will corrospond to the DPAD + this->declare_parameter("menu_button", static_cast(ButtonMap::MENU)); + menu_button_ = this->get_parameter("menu_button").as_int(); + menu_button_on_layer_ = isOnLayer(menu_button_); + menu_button_on_axis_ = isOnAxis(menu_button_); + menu_button_on_dpad_ = isOnDpad(menu_button_); + if (!recognizeButton(menu_button_)) throw std::runtime_error( + std::string("[UnitreeJoystick] Menu Button ") + std::to_string(menu_button_) + std::string(" not recognized!!") + ); + this->declare_parameter("unitree_home_button", static_cast(ButtonMap::DR1)); unitree_home_button_ = this->get_parameter("unitree_home_button").as_int(); unitree_home_button_on_layer_ = isOnLayer(unitree_home_button_); @@ -174,6 +182,7 @@ namespace obelisk { // Register publishers this->RegisterObkPublisher("pub_exec_fsm_setting", pub_exec_fsm_key_); + this->RegisterObkPublisher("pub_joy_passthrough_setting", pub_joy_passthrough_key_); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick node has been initialized."); } @@ -207,32 +216,6 @@ namespace obelisk { protected: void UpdateXHat(__attribute__((unused)) const sensor_msgs::msg::Joy& msg) override { - vel_cmd_msg vel_msg; - vel_msg.header.stamp = this->now(); - vel_msg.v_x = getAxisValue(msg, vel_cmd_x_ - AXIS_OFFSET) * v_x_scale_; - vel_msg.v_y = getAxisValue(msg, vel_cmd_y_ - AXIS_OFFSET) * v_y_scale_; - vel_msg.w_z = getAxisValue(msg, vel_cmd_yaw_ - AXIS_OFFSET) * w_z_scale_; - - this->GetPublisher(this->ctrl_key_)->publish(vel_msg); - - // Print control menu - static rclcpp::Time last_menu_press = this->now(); - if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[static_cast(ButtonMap::MENU)]) { - RCLCPP_INFO_STREAM(this->get_logger(), - "UnitreeJoystick Button Layout (XBox Controller):\n" << - "Execution Finite State Machine:\n" << - "\tHOME: " << BUTTON_NAMES.at(static_cast(unitree_home_button_)) << "\n" << - "\tDAMPING: " << BUTTON_NAMES.at(static_cast(damping_button_)) << "\n" << - "\tLOW_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(low_level_ctrl_button_)) << "\n" << - "\tHIGH_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(high_level_ctrl_button_)) << "\n" << - "High Level Velocity Commands:\n" << - "\tFront/Back: " << BUTTON_NAMES.at(static_cast(vel_cmd_x_)) << "\n" << - "\tSide/Side: " << BUTTON_NAMES.at(static_cast(vel_cmd_y_)) << "\n" << - "\tYaw Rate: " << BUTTON_NAMES.at(static_cast(vel_cmd_yaw_)) - ); - last_menu_press = this->now(); - } - // Trigger FSM // First, check for damping or estop bool commanded = false; @@ -269,6 +252,104 @@ namespace obelisk { this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command " << TRANSITION_STRINGS.at(static_cast(fsm_msg.cmd_exec_fsm_state))); } + + // Handle printing menu + static rclcpp::Time last_menu_press = this->now(); + if ((this->now() - last_menu_press).seconds() > 1 && msg.buttons[static_cast(ButtonMap::MENU)]) { + RCLCPP_INFO_STREAM(this->get_logger(), + "UnitreeJoystick Button Layout (XBox Controller):\n" << + "Execution Finite State Machine:\n" << + "\tHOME: " << BUTTON_NAMES.at(static_cast(unitree_home_button_)) << "\n" << + "\tDAMPING: " << BUTTON_NAMES.at(static_cast(damping_button_)) << "\n" << + "\tLOW_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(low_level_ctrl_button_)) << "\n" << + "\tHIGH_LEVEL_CTRL: " << BUTTON_NAMES.at(static_cast(high_level_ctrl_button_)) << "\n" << + "High Level Velocity Commands:\n" << + "\tFront/Back: " << BUTTON_NAMES.at(static_cast(vel_cmd_x_)) << "\n" << + "\tSide/Side: " << BUTTON_NAMES.at(static_cast(vel_cmd_y_)) << "\n" << + "\tYaw Rate: " << BUTTON_NAMES.at(static_cast(vel_cmd_yaw_)) + ); + last_menu_press = this->now(); + } + + // Publish velocity + vel_cmd_msg vel_msg; + vel_msg.header.stamp = this->now(); + vel_msg.v_x = getAxisValue(msg, vel_cmd_x_ - AXIS_OFFSET) * v_x_scale_; + vel_msg.v_y = getAxisValue(msg, vel_cmd_y_ - AXIS_OFFSET) * v_y_scale_; + vel_msg.w_z = getAxisValue(msg, vel_cmd_yaw_ - AXIS_OFFSET) * w_z_scale_; + + this->GetPublisher(this->ctrl_key_)->publish(vel_msg); + + // Pass through the joystick message + sensor_msgs::msg::Joy passthrough_msg = msg; + + auto zero_axis_idx = [&](int ax_idx){ + if (ax_idx >= 0 && ax_idx < static_cast(passthrough_msg.axes.size())) { + if ((ax_idx == static_cast(ButtonMap::RT) - AXIS_OFFSET) | (ax_idx == static_cast(ButtonMap::LT) - AXIS_OFFSET)) { + passthrough_msg.axes[ax_idx] = 1.0f; + } else { + passthrough_msg.axes[ax_idx] = 0.0f; + } + } + }; + auto zero_button_idx = [&](int bt_idx){ + if (bt_idx >= 0 && bt_idx < static_cast(passthrough_msg.buttons.size())) + passthrough_msg.buttons[bt_idx] = 0; + }; + + auto clear_binding = [&](int code, bool on_layer, bool on_axis, bool on_dpad){ + // If binding is layered, zero the layer button as well (prevents combo) + if (on_layer) { + if (!getButton(msg, layer_button_, false, layer_button_on_axis_, layer_button_on_dpad_)) { + return; + } + // clear the physical layer button + if (layer_button_on_axis_) { + zero_axis_idx(layer_button_ - AXIS_OFFSET); + } else if (layer_button_on_dpad_) { + zero_axis_idx(layer_button_ - AXIS_OFFSET); // dpad also lives in axes + } else { + zero_button_idx(layer_button_); + } + // reduce code to base (match how getButton() reduces) + code -= LAYER_OFFSET; + } + + if (on_axis) { + // axis/trigger input lives in axes[] + zero_axis_idx(code - AXIS_OFFSET); + return; + } + + if (on_dpad) { + // DPAD lives in axes; positive/negative handled via sign in detection + int ax = code - AXIS_OFFSET; + // For negative variants (Left/Down), normalize like getButton() does + if (code == static_cast(ButtonMap::DL) || + code == static_cast(ButtonMap::DD) || + code == static_cast(ButtonMap::DL1) || + code == static_cast(ButtonMap::DD1)) { + ax -= NEG_OFFSET; // mirror your detection + } + zero_axis_idx(ax); + return; + } + + // Plain digital button + zero_button_idx(code); + }; + + // Clear ONLY the FSM-triggering controls (leave velocity axes intact) + clear_binding(estop_, estop_on_layer_, estop_on_axis_, estop_on_dpad_); + clear_binding(menu_button_, menu_button_on_layer_, menu_button_on_axis_, menu_button_on_dpad_); + clear_binding(damping_button_, damping_button_on_layer_, damping_button_on_axis_, damping_button_on_dpad_); + clear_binding(user_pose_button_, user_pose_button_on_layer_, user_pose_button_on_axis_, user_pose_button_on_dpad_); + clear_binding(unitree_home_button_, unitree_home_button_on_layer_, unitree_home_button_on_axis_, unitree_home_button_on_dpad_); + clear_binding(low_level_ctrl_button_, low_level_ctrl_button_on_layer_, low_level_ctrl_button_on_axis_, low_level_ctrl_button_on_dpad_); + clear_binding(high_level_ctrl_button_,high_level_ctrl_button_on_layer_,high_level_ctrl_button_on_axis_,high_level_ctrl_button_on_dpad_); + + // Publish the sanitized passthrough + this->GetPublisher(this->pub_joy_passthrough_key_)->publish(passthrough_msg); } bool recognizeButton(int btn) { @@ -344,6 +425,7 @@ namespace obelisk { // Publisher key const std::string pub_exec_fsm_key_ = "pub_exec_fsm_key"; + const std::string pub_joy_passthrough_key_ = "pub_joy_passthrough_key"; // Hold velocity bounds float v_x_scale_; @@ -379,6 +461,10 @@ namespace obelisk { bool layer_button_on_dpad_; bool layer_button_on_axis_; bool estop_on_dpad_; + int menu_button_; + bool menu_button_on_layer_; + bool menu_button_on_axis_; + bool menu_button_on_dpad_; int vel_cmd_x_; int vel_cmd_y_; diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index f8037722..553c085d 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -43,7 +43,8 @@ namespace obelisk { // Set the Execution FSM into INIT exec_fsm_state_ = ExecFSMState::INIT; - high_level_ctrl_engaged_ = true; + this->declare_parameter("init_high_level", false); + high_level_ctrl_engaged_ = this->get_parameter("init_high_level").as_bool(); // Additional Publishers this->RegisterObkPublisher("pub_sensor_setting", pub_joint_state_key_); @@ -105,6 +106,10 @@ namespace obelisk { CreateUnitreeSubscribers(); + if (!high_level_ctrl_engaged_) { + ReleaseUnitreeMotionControl(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h index 8fd9b2f0..a2fa1378 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_node.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_node.h @@ -525,7 +525,7 @@ namespace obelisk { const std::string param = this->get_parameter(info.ros_param).as_string(); if (param == "") { RCLCPP_WARN_STREAM(this->get_logger(), - "Registered timer was not provided a config string! Skipping creation."); + "Registered timer was not provided a config string! Skipping creation" << key); } else { timers_[key] = info.creator(param); timers_[key]->cancel(); // Pause the timer diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 36d9afff..cfdba04e 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -122,7 +122,7 @@ onboard: history_depth: 10 # ----- IMU ----- # - ros_parameter: pub_imu_setting - topic: /obelisk/g1/croch_imu + topic: /obelisk/g1/pelvis_imu history_depth: 10 # ----- Odom ----- # - ros_parameter: pub_odom_setting diff --git a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml index 37b9ad72..cb8363a4 100644 --- a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml +++ b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml @@ -16,6 +16,10 @@ onboard: topic: /obelisk/fsm_test/high_level_ctrl history_depth: 10 callback_group: None + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/fsm_test/joy_passthrough + history_depth: 10 + callback_group: None subscribers: # ----- Joystick subscriber ----- # - ros_parameter: sub_est_setting From 02ad53a97ca6f79f12f5da4eeb4bad35a885303d Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 11:33:21 -0800 Subject: [PATCH 18/27] validated launch into low level --- obelisk/cpp/hardware/robots/unitree/unitree_interface.h | 1 + obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 5 +++++ obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml | 1 + 3 files changed, 7 insertions(+) diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 553c085d..47cc6b9e 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -107,6 +107,7 @@ namespace obelisk { CreateUnitreeSubscribers(); if (!high_level_ctrl_engaged_) { + high_level_ctrl_engaged_ = true; // Forcing Unitree state machine to transition (see ReleaseUnitreeMotionControl) ReleaseUnitreeMotionControl(); } diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index cfdba04e..a5ea7e47 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -38,6 +38,11 @@ onboard: topic: /obelisk/g1/vel_cmd history_depth: 10 callback_group: None + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/fsm_test/joy_passthrough + history_depth: 10 + callback_group: None subscribers: # ----- Joystick subscriber ----- # - ros_parameter: sub_est_setting diff --git a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml index cb8363a4..44bc36ee 100644 --- a/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml +++ b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml @@ -16,6 +16,7 @@ onboard: topic: /obelisk/fsm_test/high_level_ctrl history_depth: 10 callback_group: None + # ----- Joystick Passthrough ----- # - ros_parameter: pub_joy_passthrough_setting topic: /obelisk/fsm_test/joy_passthrough history_depth: 10 From b1a3eb99a323ecec2462d84f32ce90e89da06833 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 12:01:02 -0800 Subject: [PATCH 19/27] G1 interface user pose --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 346ec760..84f67305 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -195,7 +195,8 @@ namespace obelisk { // Compute proportion of time relative to transition duration float proportion = std::min(t / user_pose_transition_duration_, 1.0f); // Write message - for (size_t i = 0; i < num_motors_; i++) { + for (size_t j = 0; j < num_motors_; j++) { + int i = G1_JOINT_MAPPINGS.at(msg.joint_names[j]); dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(i).tau() = 0.; dds_low_command.motor_cmd().at(i).q() = (1 - proportion) * start_user_pose_[i] + proportion * user_pose_[i]; @@ -203,6 +204,17 @@ namespace obelisk { dds_low_command.motor_cmd().at(i).kp() = kp_[i]; dds_low_command.motor_cmd().at(i).kd() = kd_[i]; } + if (fixed_waist_) { + for (size_t j = 0; j < G1_EXTRA_WAIST; j++) { + int i = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[j]); + dds_low_command.motor_cmd().at(i).mode() = 0; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(i).tau() = 0; + dds_low_command.motor_cmd().at(i).q() = 0; + dds_low_command.motor_cmd().at(i).dq() = 0; + dds_low_command.motor_cmd().at(i).kp() = 0; + dds_low_command.motor_cmd().at(i).kd() = 0; + } + } } else { RCLCPP_ERROR_STREAM(this->get_logger(), "Execution FSM state not recognized in ApplyControl!"); } From c4cd39a298f7611cdc689828a44bb7709cff15b2 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 12:12:15 -0800 Subject: [PATCH 20/27] revert user pose change --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 84f67305..346ec760 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -195,8 +195,7 @@ namespace obelisk { // Compute proportion of time relative to transition duration float proportion = std::min(t / user_pose_transition_duration_, 1.0f); // Write message - for (size_t j = 0; j < num_motors_; j++) { - int i = G1_JOINT_MAPPINGS.at(msg.joint_names[j]); + for (size_t i = 0; i < num_motors_; i++) { dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(i).tau() = 0.; dds_low_command.motor_cmd().at(i).q() = (1 - proportion) * start_user_pose_[i] + proportion * user_pose_[i]; @@ -204,17 +203,6 @@ namespace obelisk { dds_low_command.motor_cmd().at(i).kp() = kp_[i]; dds_low_command.motor_cmd().at(i).kd() = kd_[i]; } - if (fixed_waist_) { - for (size_t j = 0; j < G1_EXTRA_WAIST; j++) { - int i = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[j]); - dds_low_command.motor_cmd().at(i).mode() = 0; // 1:Enable, 0:Disable - dds_low_command.motor_cmd().at(i).tau() = 0; - dds_low_command.motor_cmd().at(i).q() = 0; - dds_low_command.motor_cmd().at(i).dq() = 0; - dds_low_command.motor_cmd().at(i).kp() = 0; - dds_low_command.motor_cmd().at(i).kd() = 0; - } - } } else { RCLCPP_ERROR_STREAM(this->get_logger(), "Execution FSM state not recognized in ApplyControl!"); } From dfe06a826cfa903dd08c32ca3c7a932930445448 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 15:34:19 -0800 Subject: [PATCH 21/27] fixed default user pose --- .../hardware/robots/unitree/g1_interface.h | 157 +++++++++++++++--- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 78 +++++---- 2 files changed, 184 insertions(+), 51 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 346ec760..7fffb0fc 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -67,6 +67,97 @@ namespace obelisk { this->declare_parameter>("user_pose", user_pose_); user_pose_ = this->get_parameter("user_pose").as_double_array(); + // Handle default joint names + std::vector tmp_default_names; + if (fixed_waist_) { + tmp_default_names.assign( + G1_27DOF_JOINT_NAMES.begin(), + G1_27DOF_JOINT_NAMES.end() + ); + } else { + // Use only first num_motors_ entries + tmp_default_names.assign( + G1_FULL_JOINT_NAMES.begin(), + G1_FULL_JOINT_NAMES.begin() + num_motors_ + ); + } + this->declare_parameter>("default_joint_names", tmp_default_names); + default_joint_names_ = this->get_parameter("default_joint_names").as_string_array(); + std::unordered_set expected; + if (fixed_waist_) { + expected.reserve(G1_27DOF_JOINT_NAMES.size()); + for (const auto& name : G1_27DOF_JOINT_NAMES) { + expected.insert(name); + } + } else { + expected.reserve(G1_27DOF_JOINT_NAMES.size() + G1_EXTRA_WAIST_JOINT_NAMES.size()); + for (const auto& name : G1_27DOF_JOINT_NAMES) { + expected.insert(name); + } + for (const auto& name : G1_EXTRA_WAIST_JOINT_NAMES) { + expected.insert(name); + } + } + + if (default_joint_names_.size() != expected.size()) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "[UnitreeInterface] default_joint_names has size " + + std::to_string(default_joint_names_.size()) + + ", but expected " + std::to_string(expected.size())); + throw std::runtime_error( + "[UnitreeInterface] default_joint_names has size " + + std::to_string(default_joint_names_.size()) + + ", but expected " + std::to_string(expected.size())); + } + for (const auto& name : default_joint_names_) { + if (expected.count(name) == 0) { + RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Unknown joint in default_joint_names: " + name); + throw std::runtime_error( + "[UnitreeInterface] Unknown joint in default_joint_names: " + name); + } + } + default_joint_mapping_.clear(); + for (size_t i = 0; i < default_joint_names_.size(); ++i) { + default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); + } + + if (kp_.size() != default_joint_names_.size()) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "[UnitreeInterface] default kp has size " + + std::to_string(kp_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + throw std::runtime_error("[UnitreeInterface] default kp has size " + + std::to_string(kp_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + } + if (kd_.size() != default_joint_names_.size()) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "[UnitreeInterface] default kd has size " + + std::to_string(kd_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + throw std::runtime_error("[UnitreeInterface] default kd has size " + + std::to_string(kd_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + } + if (kd_damping_.size() != default_joint_names_.size()) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "[UnitreeInterface] default kd_damping_ has size " + + std::to_string(kd_damping_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + throw std::runtime_error("[UnitreeInterface] default kd_damping_ has size " + + std::to_string(kd_damping_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + } + if (user_pose_.size() != default_joint_names_.size()) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "[UnitreeInterface] user_pose_ has size " + + std::to_string(user_pose_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + throw std::runtime_error("[UnitreeInterface] user_pose_ has size " + + std::to_string(user_pose_.size()) + + ", but expected " + std::to_string(default_joint_names_.size())); + } + // Expose command timeout as a ros parameter this->declare_parameter("command_timeout", 2.0f); command_timeout_ = this->get_parameter("command_timeout").as_double(); @@ -167,24 +258,25 @@ namespace obelisk { use_default_gains = false; } - for (size_t j = 0; j < G1_27DOF; j++) { // Only go through the non-hand motors - int i = G1_JOINT_MAPPINGS.at(msg.joint_names[j]); - dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable - dds_low_command.motor_cmd().at(i).tau() = msg.feed_forward[j]; - dds_low_command.motor_cmd().at(i).q() = msg.pos_target[j]; - dds_low_command.motor_cmd().at(i).dq() = msg.vel_target[j]; - dds_low_command.motor_cmd().at(i).kp() = use_default_gains ? kp_[i] : msg.kp[j]; - dds_low_command.motor_cmd().at(i).kd() = use_default_gains ? kd_[i] : msg.kd[j]; + for (size_t msg_ind = 0; msg_ind < G1_27DOF; msg_ind++) { // Only go through the non-hand motors + int uni_ind = G1_JOINT_MAPPINGS.at(msg.joint_names[msg_ind]); + int def_ind = default_joint_mapping_.at(msg.joint_names[msg_ind]); + dds_low_command.motor_cmd().at(uni_ind).mode() = 1; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(uni_ind).tau() = msg.feed_forward[msg_ind]; + dds_low_command.motor_cmd().at(uni_ind).q() = msg.pos_target[msg_ind]; + dds_low_command.motor_cmd().at(uni_ind).dq() = msg.vel_target[msg_ind]; + dds_low_command.motor_cmd().at(uni_ind).kp() = use_default_gains ? kp_[def_ind] : msg.kp[msg_ind]; + dds_low_command.motor_cmd().at(uni_ind).kd() = use_default_gains ? kd_[def_ind] : msg.kd[msg_ind]; } if (fixed_waist_) { - for (size_t j = 0; j < G1_EXTRA_WAIST; j++) { - int i = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[j]); - dds_low_command.motor_cmd().at(i).mode() = 0; // 1:Enable, 0:Disable - dds_low_command.motor_cmd().at(i).tau() = 0; - dds_low_command.motor_cmd().at(i).q() = 0; - dds_low_command.motor_cmd().at(i).dq() = 0; - dds_low_command.motor_cmd().at(i).kp() = 0; - dds_low_command.motor_cmd().at(i).kd() = 0; + for (size_t waist_ind = 0; waist_ind < G1_EXTRA_WAIST; waist_ind++) { + int uni_ind = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[waist_ind]); + dds_low_command.motor_cmd().at(uni_ind).mode() = 0; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(uni_ind).tau() = 0; + dds_low_command.motor_cmd().at(uni_ind).q() = 0; + dds_low_command.motor_cmd().at(uni_ind).dq() = 0; + dds_low_command.motor_cmd().at(uni_ind).kp() = 0; + dds_low_command.motor_cmd().at(uni_ind).kd() = 0; } } // ------------------------ Execution FSM: Home ------------------------ // @@ -195,13 +287,25 @@ namespace obelisk { // Compute proportion of time relative to transition duration float proportion = std::min(t / user_pose_transition_duration_, 1.0f); // Write message - for (size_t i = 0; i < num_motors_; i++) { - dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable - dds_low_command.motor_cmd().at(i).tau() = 0.; - dds_low_command.motor_cmd().at(i).q() = (1 - proportion) * start_user_pose_[i] + proportion * user_pose_[i]; - dds_low_command.motor_cmd().at(i).dq() = 0.; - dds_low_command.motor_cmd().at(i).kp() = kp_[i]; - dds_low_command.motor_cmd().at(i).kd() = kd_[i]; + for (size_t def_ind = 0; def_ind < default_joint_names_.size(); def_ind++) { // Only go through the non-hand motors + int uni_ind = G1_JOINT_MAPPINGS.at(default_joint_names_[def_ind]); + dds_low_command.motor_cmd().at(uni_ind).mode() = 1; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(uni_ind).tau() = 0.; + dds_low_command.motor_cmd().at(uni_ind).q() = (1 - proportion) * start_user_pose_[uni_ind] + proportion * user_pose_[def_ind]; + dds_low_command.motor_cmd().at(uni_ind).dq() = 0.; + dds_low_command.motor_cmd().at(uni_ind).kp() = kp_[def_ind]; + dds_low_command.motor_cmd().at(uni_ind).kd() = kd_[def_ind]; + } + if (fixed_waist_) { + for (size_t waist_ind = 0; waist_ind < G1_EXTRA_WAIST; waist_ind++) { + int uni_ind = G1_JOINT_MAPPINGS.at(G1_EXTRA_WAIST_JOINT_NAMES[waist_ind]); + dds_low_command.motor_cmd().at(uni_ind).mode() = 0; // 1:Enable, 0:Disable + dds_low_command.motor_cmd().at(uni_ind).tau() = 0; + dds_low_command.motor_cmd().at(uni_ind).q() = 0; + dds_low_command.motor_cmd().at(uni_ind).dq() = 0; + dds_low_command.motor_cmd().at(uni_ind).kp() = 0; + dds_low_command.motor_cmd().at(uni_ind).kd() = 0; + } } } else { RCLCPP_ERROR_STREAM(this->get_logger(), "Execution FSM state not recognized in ApplyControl!"); @@ -233,6 +337,11 @@ namespace obelisk { joint_state.joint_names.at(i) = G1_FULL_JOINT_NAMES[i]; joint_state.joint_pos.at(i) = low_state.motor_state()[i].q(); joint_state.joint_vel.at(i) = low_state.motor_state()[i].dq(); + { + std::lock_guard lock(joint_mutex_); + joint_pos_[i] = low_state.motor_state()[i].q(); + joint_vel_[i] = low_state.motor_state()[i].dq(); + } } this->GetPublisher(pub_joint_state_key_)->publish(joint_state); @@ -498,6 +607,8 @@ namespace obelisk { std::vector joint_vel_; // Local copy of joint positions std::vector start_user_pose_; // For transitioning to home position std::vector user_pose_; // home position + std::vector default_joint_names_; + std::unordered_map default_joint_mapping_; std::mutex joint_mutex_; // mutex for copying joint positions and velocities g1::LocoClient loco_client_; // Locomotion Client for high level control diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index a5ea7e47..376d6963 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -85,38 +85,60 @@ onboard: executable: obelisk_unitree_g1_hardware params: network_interface_name: enx6c1ff724e92e + default_joint_names: [ + "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", + "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", + "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", + "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", + "waist_yaw_joint", + "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", + "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", + "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", + ] + user_pose: [ + -0.25, 0, 0, + 0.46, -0.25, 0, + -0.25, 0, 0, + 0.46, -0.25, 0, + 0, + 0.07, 0.24, 0, 1.39, + 0, 0, 0, + 0.07, -0.24, 0, 1.39, + 0, 0, 0 + ] default_kp: [ - 10., 10., 10., # L hip - 10., 10., 10., # L knee, ankle - 10., 10., 10., # R hip - 10., 10., 10., # R knee, ankle - 10., #10., 10., # Waist - 5., 5., 5., # L shoulder - 5., 5., 5., 5., # L elbow, wrist - 5., 5., 5., # R shoulder - 5., 5., 5., 5., # R elbow, wrist - ] + 30., 30., 30., # L hip + 30., 30., 30., # L knee, ankle + 30., 30., 30., # R hip + 30., 30., 30., # R knee, ankle + 30., # Waist + 10., 10., 10., 10., # L shoulder, elbow + 10., 10., 10., # L wrist + 10., 10., 10., 10., # R shoulder, elbow + 10., 10., 10., # R wrist + ] default_kd: [ - 1., 1., 1., # L hip - 1., 1., 1., # L knee, ankle - 1., 1., 1., # R hip - 1., 1., 1., # R knee, ankle - 1., #1., 1., # Waist - 1., 1., 1., # L shoulder - 1., 1., 1., 1., # L elbow, wrist - 1., 1., 1., # R shoulder - 1., 1., 1., 1., # R elbow, wrist + 3., 3., 3., # L hip + 3., 3., 3., # L knee, ankle + 3., 3., 3., # R hip + 3., 3., 3., # R knee, ankle + 3., # Waist + 2., 2., 2., 2., # L shoulder, elbow + 2., 2., 2., # L wrist + 2., 2., 2., 2., # R shoulder, elbow + 2., 2., 2., # R wrist ] default_kd_damping: [ - 10., 10., 10., # L hip - 10., 10., 10., # L knee, ankle - 10., 10., 10., # R hip - 10., 10., 10., # R knee, ankle - 10., #10., 10., # Waist - 10., 10., 10., # L shoulder - 10., 10., 10., 10., # L elbow, wrist - 10., 10., 10., # R shoulder - 10., 10., 10., 10., # R elbow, wrist + 10., 10., 10., # L hip + 10., 10., 10., # L knee, ankle + 10., 10., 10., # R hip + 10., 10., 10., # R knee, ankle + 10., # Waist + 10., 10., 10., 10., # L shoulder, elbow + 10., 10., 10., # L wrist + 10., 10., 10., 10., # R shoulder, elbow + 10., 10., 10., # R wrist ] # ================== # callback_groups: From 89c5474e10e4124707a247bd1af4ec8fb3eeff5b Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 18:39:59 -0800 Subject: [PATCH 22/27] publishing change for fixed waist --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 7fffb0fc..6312cc26 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -333,15 +333,23 @@ namespace obelisk { joint_state.joint_vel.resize(G1_27DOF + G1_EXTRA_WAIST); joint_state.joint_names.resize(G1_27DOF + G1_EXTRA_WAIST); + size_t ind = 0; for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { - joint_state.joint_names.at(i) = G1_FULL_JOINT_NAMES[i]; - joint_state.joint_pos.at(i) = low_state.motor_state()[i].q(); - joint_state.joint_vel.at(i) = low_state.motor_state()[i].dq(); + if (fixed_waist_ && std::find( + G1_EXTRA_WAIST_JOINT_NAMES.begin(), + G1_EXTRA_WAIST_JOINT_NAMES.end(), + G1_FULL_JOINT_NAMES[i]) != G1_EXTRA_WAIST_JOINT_NAMES.end()) { + continue; // if the waist is fixed, don't publish the two waist joints (like mujoco...) + } + joint_state.joint_names.at(ind) = G1_FULL_JOINT_NAMES[i]; + joint_state.joint_pos.at(ind) = low_state.motor_state()[i].q(); + joint_state.joint_vel.at(ind) = low_state.motor_state()[i].dq(); { std::lock_guard lock(joint_mutex_); joint_pos_[i] = low_state.motor_state()[i].q(); joint_vel_[i] = low_state.motor_state()[i].dq(); } + ind++; } this->GetPublisher(pub_joint_state_key_)->publish(joint_state); From 81bcec9d470d142331828d7baed7eba405a32b04 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 19:02:23 -0800 Subject: [PATCH 23/27] removed hands, simplified waist --- .../hardware/robots/unitree/g1_interface.h | 264 +++++++----------- 1 file changed, 102 insertions(+), 162 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 6312cc26..2ca1a22a 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -22,28 +22,17 @@ namespace obelisk { class G1Interface : public ObeliskUnitreeInterface { public: G1Interface(const std::string& node_name) - : ObeliskUnitreeInterface(node_name, "g1"), use_hands_(false), fixed_waist_(true), mode_pr_(AnkleMode::PR), mode_machine_(0) { + : ObeliskUnitreeInterface(node_name, "g1"), fixed_waist_(true), mode_pr_(AnkleMode::PR), mode_machine_(0) { // Expose duration of transition to home position as ros parameter this->declare_parameter("user_pose_transition_duration", 1.); user_pose_transition_duration_ = this->get_parameter("user_pose_transition_duration").as_double(); - this->declare_parameter("use_hands", false); - use_hands_ = this->get_parameter("use_hands").as_bool(); - - if (use_hands_) { - RCLCPP_ERROR_STREAM(this->get_logger(), "use_hands_ = True not supported for g1 interface."); - } - this->declare_parameter("fixed_waist", true); fixed_waist_ = this->get_parameter("fixed_waist").as_bool(); // Set G1 specific values num_motors_ = G1_27DOF; - if (use_hands_) { - num_motors_ += G1_HAND_MOTOR_NUM; - } - if (!fixed_waist_) { num_motors_ += G1_EXTRA_WAIST; } @@ -58,7 +47,6 @@ namespace obelisk { } // Setup local variables - // TODO: For now we are not supporting the hands here joint_pos_.resize(G1_27DOF + G1_EXTRA_WAIST); joint_vel_.resize(G1_27DOF + G1_EXTRA_WAIST); start_user_pose_.resize(G1_27DOF + G1_EXTRA_WAIST); @@ -68,95 +56,9 @@ namespace obelisk { user_pose_ = this->get_parameter("user_pose").as_double_array(); // Handle default joint names - std::vector tmp_default_names; - if (fixed_waist_) { - tmp_default_names.assign( - G1_27DOF_JOINT_NAMES.begin(), - G1_27DOF_JOINT_NAMES.end() - ); - } else { - // Use only first num_motors_ entries - tmp_default_names.assign( - G1_FULL_JOINT_NAMES.begin(), - G1_FULL_JOINT_NAMES.begin() + num_motors_ - ); - } - this->declare_parameter>("default_joint_names", tmp_default_names); + std::vector default_names_vec(G1_27DOF_JOINT_NAMES.begin(), G1_27DOF_JOINT_NAMES.end()); + this->declare_parameter>("default_joint_names", default_names_vec); default_joint_names_ = this->get_parameter("default_joint_names").as_string_array(); - std::unordered_set expected; - if (fixed_waist_) { - expected.reserve(G1_27DOF_JOINT_NAMES.size()); - for (const auto& name : G1_27DOF_JOINT_NAMES) { - expected.insert(name); - } - } else { - expected.reserve(G1_27DOF_JOINT_NAMES.size() + G1_EXTRA_WAIST_JOINT_NAMES.size()); - for (const auto& name : G1_27DOF_JOINT_NAMES) { - expected.insert(name); - } - for (const auto& name : G1_EXTRA_WAIST_JOINT_NAMES) { - expected.insert(name); - } - } - - if (default_joint_names_.size() != expected.size()) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "[UnitreeInterface] default_joint_names has size " + - std::to_string(default_joint_names_.size()) + - ", but expected " + std::to_string(expected.size())); - throw std::runtime_error( - "[UnitreeInterface] default_joint_names has size " + - std::to_string(default_joint_names_.size()) + - ", but expected " + std::to_string(expected.size())); - } - for (const auto& name : default_joint_names_) { - if (expected.count(name) == 0) { - RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Unknown joint in default_joint_names: " + name); - throw std::runtime_error( - "[UnitreeInterface] Unknown joint in default_joint_names: " + name); - } - } - default_joint_mapping_.clear(); - for (size_t i = 0; i < default_joint_names_.size(); ++i) { - default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); - } - - if (kp_.size() != default_joint_names_.size()) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "[UnitreeInterface] default kp has size " + - std::to_string(kp_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - throw std::runtime_error("[UnitreeInterface] default kp has size " + - std::to_string(kp_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - } - if (kd_.size() != default_joint_names_.size()) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "[UnitreeInterface] default kd has size " + - std::to_string(kd_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - throw std::runtime_error("[UnitreeInterface] default kd has size " + - std::to_string(kd_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - } - if (kd_damping_.size() != default_joint_names_.size()) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "[UnitreeInterface] default kd_damping_ has size " + - std::to_string(kd_damping_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - throw std::runtime_error("[UnitreeInterface] default kd_damping_ has size " + - std::to_string(kd_damping_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - } - if (user_pose_.size() != default_joint_names_.size()) { - RCLCPP_ERROR_STREAM(this->get_logger(), - "[UnitreeInterface] user_pose_ has size " + - std::to_string(user_pose_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - throw std::runtime_error("[UnitreeInterface] user_pose_ has size " + - std::to_string(user_pose_.size()) + - ", but expected " + std::to_string(default_joint_names_.size())); - } // Expose command timeout as a ros parameter this->declare_parameter("command_timeout", 2.0f); @@ -165,12 +67,6 @@ namespace obelisk { CMD_TOPIC_ = "rt/lowcmd"; STATE_TOPIC_ = "rt/lowstate"; - // TODO: Add support for rt/lowstate_doubleimu - - // TODO: Consider exposing AB mode - - // TODO: Deal with hands - RCLCPP_INFO_STREAM(this->get_logger(), "G1 configured as: \n" << "\tHands: " << use_hands_ << "\n\tFixed waist: " << fixed_waist_); VerifyParameters(); @@ -258,7 +154,7 @@ namespace obelisk { use_default_gains = false; } - for (size_t msg_ind = 0; msg_ind < G1_27DOF; msg_ind++) { // Only go through the non-hand motors + for (size_t msg_ind = 0; msg_ind < num_motors_; msg_ind++) { // Only go through the non-hand motors int uni_ind = G1_JOINT_MAPPINGS.at(msg.joint_names[msg_ind]); int def_ind = default_joint_mapping_.at(msg.joint_names[msg_ind]); dds_low_command.motor_cmd().at(uni_ind).mode() = 1; // 1:Enable, 0:Disable @@ -287,7 +183,7 @@ namespace obelisk { // Compute proportion of time relative to transition duration float proportion = std::min(t / user_pose_transition_duration_, 1.0f); // Write message - for (size_t def_ind = 0; def_ind < default_joint_names_.size(); def_ind++) { // Only go through the non-hand motors + for (size_t def_ind = 0; def_ind < num_motors_; def_ind++) { // Only go through the non-hand motors int uni_ind = G1_JOINT_MAPPINGS.at(default_joint_names_[def_ind]); dds_low_command.motor_cmd().at(uni_ind).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(uni_ind).tau() = 0.; @@ -329,26 +225,27 @@ namespace obelisk { // Joints obelisk_sensor_msgs::msg::ObkJointEncoders joint_state; joint_state.header.stamp = this->now(); - joint_state.joint_pos.resize(G1_27DOF + G1_EXTRA_WAIST); - joint_state.joint_vel.resize(G1_27DOF + G1_EXTRA_WAIST); - joint_state.joint_names.resize(G1_27DOF + G1_EXTRA_WAIST); + joint_state.joint_pos.resize(num_motors_); + joint_state.joint_vel.resize(num_motors_); + joint_state.joint_names.resize(num_motors_); size_t ind = 0; for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { + { + std::lock_guard lock(joint_mutex_); + joint_pos_[i] = low_state.motor_state()[i].q(); + joint_vel_[i] = low_state.motor_state()[i].dq(); + } if (fixed_waist_ && std::find( G1_EXTRA_WAIST_JOINT_NAMES.begin(), G1_EXTRA_WAIST_JOINT_NAMES.end(), - G1_FULL_JOINT_NAMES[i]) != G1_EXTRA_WAIST_JOINT_NAMES.end()) { + G1_29DOF_JOINT_NAMES[i]) != G1_EXTRA_WAIST_JOINT_NAMES.end()) { continue; // if the waist is fixed, don't publish the two waist joints (like mujoco...) } - joint_state.joint_names.at(ind) = G1_FULL_JOINT_NAMES[i]; + joint_state.joint_names.at(ind) = G1_29DOF_JOINT_NAMES[i]; joint_state.joint_pos.at(ind) = low_state.motor_state()[i].q(); joint_state.joint_vel.at(ind) = low_state.motor_state()[i].dq(); - { - std::lock_guard lock(joint_mutex_); - joint_pos_[i] = low_state.motor_state()[i].q(); - joint_vel_[i] = low_state.motor_state()[i].dq(); - } + ind++; } @@ -441,7 +338,7 @@ namespace obelisk { } else { // use low level damping LowCmd_ dds_low_command; - for (size_t j = 0; j < G1_27DOF; j++) { // Only go through the non-hand motors + for (size_t j = 0; j < num_motors_; j++) { // Only go through the non-hand motors int i = G1_JOINT_MAPPINGS.at(joint_names_[j]); dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(i).tau() = 0; @@ -528,10 +425,10 @@ namespace obelisk { motor_data_log_ << "# Timestamps below are relative to startup in seconds\n"; motor_data_log_ << "timestamp_sec"; for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { - motor_data_log_ << ",temp1_" << G1_FULL_JOINT_NAMES[i] - << ",temp2_" << G1_FULL_JOINT_NAMES[i] - << ",tau_est_" << G1_FULL_JOINT_NAMES[i] - << ",motorstate_" << G1_FULL_JOINT_NAMES[i]; + motor_data_log_ << ",temp1_" << G1_29DOF_JOINT_NAMES[i] + << ",temp2_" << G1_29DOF_JOINT_NAMES[i] + << ",tau_est_" << G1_29DOF_JOINT_NAMES[i] + << ",motorstate_" << G1_29DOF_JOINT_NAMES[i]; } motor_data_log_ << "\n"; motor_data_log_.flush(); @@ -547,7 +444,7 @@ namespace obelisk { if (joint_names_log.is_open()) { joint_names_log << "Joint order for logged data:\n"; for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { - joint_names_log << i << ": " << G1_FULL_JOINT_NAMES[i] << "\n"; + joint_names_log << i << ": " << G1_29DOF_JOINT_NAMES[i] << "\n"; } joint_names_log.close(); RCLCPP_INFO_STREAM(this->get_logger(), "Joint names file created: " << joint_names_file); @@ -623,16 +520,14 @@ namespace obelisk { std::shared_ptr motion_switcher_client_; // Robot State Client for high level/low level handoff std::vector default_user_pose_ = { - 0.3, 0, 0, 0.52, -0.23, 0, // Left Leg + -0.3, 0, 0, 0.52, -0.23, 0, // Left Leg -0.3, 0, 0, 0.52, -0.23, 0, // Right Leg - 0, 0, 0.03, // Waist + 0, // Waist 0, 0, 0, 0.2, 0, 0, 0, // Left arm 0, 0, 0, 0.2, 0, 0, 0, // Right arm - 0, 0, 0, 0, 0, 0, 0, // Left hand - 0, 0, 0, 0, 0, 0, 0 // Right hand }; // Default home position - const std::array G1_FULL_JOINT_NAMES = { + const std::array G1_29DOF_JOINT_NAMES = { "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", @@ -662,20 +557,6 @@ namespace obelisk { "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", - "left_hand_thumb_0_joint", // ----- Hand Start ----- // - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint" }; const std::array G1_27DOF_JOINT_NAMES = { @@ -713,23 +594,6 @@ namespace obelisk { "waist_pitch_joint" }; - const std::array G1_HAND_JOINT_NAMES = { - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint" - }; - const std::map G1_JOINT_MAPPINGS = { {"left_hip_pitch_joint", 0}, {"left_hip_roll_joint", 1}, @@ -762,4 +626,80 @@ namespace obelisk { {"right_wrist_yaw_joint", 28} }; }; -} // namespace obelisk \ No newline at end of file +} // namespace obelisk + + +// std::unordered_set expected; +// if (fixed_waist_) { +// expected.reserve(G1_27DOF_JOINT_NAMES.size()); +// for (const auto& name : G1_27DOF_JOINT_NAMES) { +// expected.insert(name); +// } +// } else { +// expected.reserve(G1_27DOF_JOINT_NAMES.size() + G1_EXTRA_WAIST_JOINT_NAMES.size()); +// for (const auto& name : G1_27DOF_JOINT_NAMES) { +// expected.insert(name); +// } +// for (const auto& name : G1_EXTRA_WAIST_JOINT_NAMES) { +// expected.insert(name); +// } +// } + +// if (default_joint_names_.size() != expected.size()) { +// RCLCPP_ERROR_STREAM(this->get_logger(), +// "[UnitreeInterface] default_joint_names has size " + +// std::to_string(default_joint_names_.size()) + +// ", but expected " + std::to_string(expected.size())); +// throw std::runtime_error( +// "[UnitreeInterface] default_joint_names has size " + +// std::to_string(default_joint_names_.size()) + +// ", but expected " + std::to_string(expected.size())); +// } +// for (const auto& name : default_joint_names_) { +// if (expected.count(name) == 0) { +// RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Unknown joint in default_joint_names: " + name); +// throw std::runtime_error( +// "[UnitreeInterface] Unknown joint in default_joint_names: " + name); +// } +// } +// default_joint_mapping_.clear(); +// for (size_t i = 0; i < default_joint_names_.size(); ++i) { +// default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); +// } + +// if (kp_.size() != default_joint_names_.size()) { +// RCLCPP_ERROR_STREAM(this->get_logger(), +// "[UnitreeInterface] default kp has size " + +// std::to_string(kp_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// throw std::runtime_error("[UnitreeInterface] default kp has size " + +// std::to_string(kp_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// } +// if (kd_.size() != default_joint_names_.size()) { +// RCLCPP_ERROR_STREAM(this->get_logger(), +// "[UnitreeInterface] default kd has size " + +// std::to_string(kd_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// throw std::runtime_error("[UnitreeInterface] default kd has size " + +// std::to_string(kd_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// } +// if (kd_damping_.size() != default_joint_names_.size()) { +// RCLCPP_ERROR_STREAM(this->get_logger(), +// "[UnitreeInterface] default kd_damping_ has size " + +// std::to_string(kd_damping_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// throw std::runtime_error("[UnitreeInterface] default kd_damping_ has size " + +// std::to_string(kd_damping_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// } +// if (user_pose_.size() != default_joint_names_.size()) { +// RCLCPP_ERROR_STREAM(this->get_logger(), +// "[UnitreeInterface] user_pose_ has size " + +// std::to_string(user_pose_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// throw std::runtime_error("[UnitreeInterface] user_pose_ has size " + +// std::to_string(user_pose_.size()) + +// ", but expected " + std::to_string(default_joint_names_.size())); +// } \ No newline at end of file From 15734c4f5834d19ef3aa9f54bb883c22b468c792 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 20:57:49 -0800 Subject: [PATCH 24/27] added back default_joints --- .../hardware/robots/unitree/g1_interface.h | 8 ++++-- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 28 +++++++++---------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 2ca1a22a..3dfcb097 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -59,6 +59,10 @@ namespace obelisk { std::vector default_names_vec(G1_27DOF_JOINT_NAMES.begin(), G1_27DOF_JOINT_NAMES.end()); this->declare_parameter>("default_joint_names", default_names_vec); default_joint_names_ = this->get_parameter("default_joint_names").as_string_array(); + default_joint_mapping_.clear(); + for (size_t i = 0; i < default_joint_names_.size(); ++i) { + default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); + } // Expose command timeout as a ros parameter this->declare_parameter("command_timeout", 2.0f); @@ -189,7 +193,7 @@ namespace obelisk { dds_low_command.motor_cmd().at(uni_ind).tau() = 0.; dds_low_command.motor_cmd().at(uni_ind).q() = (1 - proportion) * start_user_pose_[uni_ind] + proportion * user_pose_[def_ind]; dds_low_command.motor_cmd().at(uni_ind).dq() = 0.; - dds_low_command.motor_cmd().at(uni_ind).kp() = kp_[def_ind]; + dds_low_command.motor_cmd().at(uni_ind).kp() = (1 - proportion) * 10 + proportion * kp_[def_ind]; dds_low_command.motor_cmd().at(uni_ind).kd() = kd_[def_ind]; } if (fixed_waist_) { @@ -513,7 +517,7 @@ namespace obelisk { std::vector start_user_pose_; // For transitioning to home position std::vector user_pose_; // home position std::vector default_joint_names_; - std::unordered_map default_joint_mapping_; + std::map default_joint_mapping_; std::mutex joint_mutex_; // mutex for copying joint positions and velocities g1::LocoClient loco_client_; // Locomotion Client for high level control diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 376d6963..999a2b86 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -108,22 +108,22 @@ onboard: 0, 0, 0 ] default_kp: [ - 30., 30., 30., # L hip - 30., 30., 30., # L knee, ankle - 30., 30., 30., # R hip - 30., 30., 30., # R knee, ankle - 30., # Waist - 10., 10., 10., 10., # L shoulder, elbow - 10., 10., 10., # L wrist - 10., 10., 10., 10., # R shoulder, elbow - 10., 10., 10., # R wrist + 100., 100., 100., # L hip + 150., 100., 100., # L knee, ankle + 100., 100., 100., # R hip + 150., 100., 100., # R knee, ankle + 100., # Waist + 50., 50., 40., 40., # L shoulder, elbow + 30., 30., 30., # L wrist + 50., 50., 40., 40., # R shoulder, elbow + 30., 30., 30., # R wrist ] default_kd: [ - 3., 3., 3., # L hip - 3., 3., 3., # L knee, ankle - 3., 3., 3., # R hip - 3., 3., 3., # R knee, ankle - 3., # Waist + 2., 2., 2., # L hip + 4., 2., 2., # L knee, ankle + 2., 2., 2., # R hip + 4., 2., 2., # R knee, ankle + 2., # Waist 2., 2., 2., 2., # L shoulder, elbow 2., 2., 2., # L wrist 2., 2., 2., 2., # R shoulder, elbow From ef8e5f863630459aa2a4c1a39d268d0e9b72ab70 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 22:36:29 -0800 Subject: [PATCH 25/27] programmatic network interface --- .../robots/unitree/unitree_interface.h | 45 ++++++++++++++----- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 2 +- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 47cc6b9e..9c8e029e 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -39,6 +39,12 @@ namespace obelisk { // Get network interface name as a parameter this->declare_parameter("network_interface_name", ""); network_interface_name_ = this->get_parameter("network_interface_name").as_string(); + if (network_interface_name_ == "") { + this->declare_parameter("target_ip", "192.168.123.222"); + std::string target_ip = this->get_parameter("target_ip").as_string(); + RCLCPP_INFO_STREAM(this->get_logger(), "Target IP: " << target_ip); + GetInterfaceName(target_ip, network_interface_name_); + } RCLCPP_INFO_STREAM(this->get_logger(), "Network interface: " << network_interface_name_); // Set the Execution FSM into INIT @@ -114,16 +120,6 @@ namespace obelisk { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - // Set of legal transitions for the execution FSM - // const std::unordered_map> TRANSITIONS = { - // {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Init -> Home, Init -> Damp - // {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::UNITREE_VEL_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp - // {ExecFSMState::USER_POSE, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Pose -> Home, Pose -> Low, Pose -> Damp - // {ExecFSMState::USER_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Low -> Home, Low -> Pose, Low -> Damp - // {ExecFSMState::UNITREE_VEL_CTRL, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // High -> Home, High -> Damp - // {ExecFSMState::DAMPING, {ExecFSMState::UNITREE_HOME, ExecFSMState::USER_POSE, ExecFSMState::ESTOP}}, // Damp -> Home, Damp -> Pose - // {ExecFSMState::ESTOP, {}} // Estop (None) - // }; const std::unordered_map> TRANSITIONS = { {ExecFSMState::INIT, {ExecFSMState::UNITREE_HOME, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Init -> Home, Init -> Damp {ExecFSMState::UNITREE_HOME, {ExecFSMState::USER_POSE, ExecFSMState::USER_CTRL, ExecFSMState::UNITREE_VEL_CTRL, ExecFSMState::DAMPING, ExecFSMState::ESTOP}}, // Home -> Pose, Home -> Low, Home -> High, Home -> Damp @@ -324,5 +320,34 @@ namespace obelisk { private: + void GetInterfaceName(const std::string& target_ip, std::string& network_interface_name_) { + struct ifaddrs *ifaddr = nullptr; + if (getifaddrs(&ifaddr) == -1) { + RCLCPP_ERROR_STREAM(this->get_logger(), "if_addr error."); + throw std::runtime_error("ifadd error."); + } + + bool found = false; + for (auto *ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next) { + if (!ifa->ifa_addr || ifa->ifa_addr->sa_family != AF_INET) + continue; + + char addr[INET_ADDRSTRLEN]; + auto *sa = reinterpret_cast(ifa->ifa_addr); + inet_ntop(AF_INET, &sa->sin_addr, addr, sizeof(addr)); + + if (target_ip == addr) { + network_interface_name_ = ifa->ifa_name; + found = true; + break; + } + } + + freeifaddrs(ifaddr); + if (!found) { + RCLCPP_ERROR_STREAM(this->get_logger(), "No interface found with IP: " + target_ip); + throw std::runtime_error("No interface found with IP: " + target_ip); + } + } }; } // namespace obelisk \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 999a2b86..7d53a1d7 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -84,7 +84,7 @@ onboard: pkg: obelisk_unitree_cpp executable: obelisk_unitree_g1_hardware params: - network_interface_name: enx6c1ff724e92e + # network_interface_name: enx6c1ff724e92e default_joint_names: [ "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", From bd5df47eaa768f35a1c30286bff0c0dea5dfafec Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 23:08:49 -0800 Subject: [PATCH 26/27] docs --- docs/source/unitree_interfaces.md | 31 +++- .../cpp/hardware/include/unitree_joystick.h | 8 +- .../hardware/robots/unitree/g1_interface.h | 151 ++++++++---------- obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml | 2 +- 4 files changed, 92 insertions(+), 100 deletions(-) diff --git a/docs/source/unitree_interfaces.md b/docs/source/unitree_interfaces.md index ca1fc63b..4a68a107 100644 --- a/docs/source/unitree_interfaces.md +++ b/docs/source/unitree_interfaces.md @@ -26,7 +26,7 @@ To avoid having the default FSM conflict with any user-specified FSM operating o - DPad Up (L1): UNITREE VEL CTRL - DPad Down (L1): USER CTRL -where L1 refers to Layer 1, i.e. the layer button must be held down. +where L1 refers to Layer 1, i.e. the layer button must be held down. Additionally, all buttons not passed to the fsm are passed through on a separate joystick node (`/obelisk/g1/joy_passthrough` by default). It is recommended that the user subscribe to this topic to use the controller, rather than the joystick topic (default `/obelisk/g1/joy`), to avoid interference with the fsm. All of these buttons can be remapped as desired in the setup yaml. Here is a complete yaml, with all available options (omitted options will take default values): @@ -39,8 +39,8 @@ joystick: v_x_scale: 0.5 v_y_scale: 0.5 w_z_scale: 0.5 - unitree_home_button: 116 // DR1 - user_pose_button: 127 // DL1 + unitree_home_button: 116 // DL1 + user_pose_button: 127 // DR1 low_level_ctrl_button: 128 // DD1 high_level_ctrl_button: 117 // DU1 damping_button: 12 // LT @@ -61,8 +61,8 @@ X = 3, Y = 2, A = 0, B = 1, -DL = 27 // 6 + AXIS_OFFSET + NEG_OFFSET, -DR = 16 // 6 + AXIS_OFFSET, +DR = 27 // 6 + AXIS_OFFSET + NEG_OFFSET, +DL = 16 // 6 + AXIS_OFFSET, DU = 17 // 7 + AXIS_OFFSET, DD = 28 // 7 + AXIS_OFFSET + NEG_OFFSET, LX = 10 // 0 + AXIS_OFFSET, @@ -76,10 +76,25 @@ X1 = 103 // X + LAYER_OFFSET, Y1 = 102 // Y + LAYER_OFFSET, A1 = 100 // A + LAYER_OFFSET, B1 = 101 // B + LAYER_OFFSET, -DL1 = 127 // DL + LAYER_OFFSET, -DR1 = 116 // DR + LAYER_OFFSET, +DR1 = 127 // DL + LAYER_OFFSET, +DL1 = 116 // DR + LAYER_OFFSET, DU1 = 117 // DU + LAYER_OFFSET, DD1 = 128 // DD + LAYER_OFFSET, ``` -Note that to avoid conflicts between `button` indices and `axis` indices (and issues with the DPad), we have added offsets `LAYER_OFFSET = 100`, to buttons on Layer 1, `AXIS_OFFSET = 10` to axes, and `NEG_OFFSET = 11` to elements of the DPad which report negative values when pressed. \ No newline at end of file +Note that to avoid conflicts between `button` indices and `axis` indices (and issues with the DPad), we have added offsets `LAYER_OFFSET = 100`, to buttons on Layer 1, `AXIS_OFFSET = 10` to axes, and `NEG_OFFSET = 11` to elements of the DPad which report negative values when pressed. + + +## Standard Pipeline for High Level Control +1. Ensure the relevant flag is `robot -> is simulated: False -> params: -> init_high_level: True`. +2. Turn the robot on. +3. Once robot signifies ready state (zero torque mode on G1 humanoid, standing up on Go2 quadruped), run fsm. +4. Start with INIT -> DAMPING, and follow up with DAMPING -> UNITREE_HOME (for the G1), or proceed directly with INIT -> UNITREE_HOME (for the Go2). +5. Proceed with UNITREE_HOME -> UNITREE_VEL for velocity control with the unitree default locomotion controllers. + +## Standard Pipeline for Low Level Control +1. Either omit or ensure the relevant flag is `robot -> is simulated: False -> params: -> init_high_level: False`. +2. Turn the robot on. +3. Once robot signifies ready state (zero torque mode on G1 humanoid, standing up on Go2 quadruped), run fsm. +4. Start with INIT -> DAMPING, and follow up with DAMPING -> USER_POSE (for the G1), or proceed directly with INIT -> USER_POSE (for the Go2). +5. Proceed with USER_POSE -> USER_CTRL for user defined controllers. \ No newline at end of file diff --git a/obelisk/cpp/hardware/include/unitree_joystick.h b/obelisk/cpp/hardware/include/unitree_joystick.h index 8f95fc51..8d441020 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -30,8 +30,8 @@ namespace obelisk { Y = 2, A = 0, B = 1, - DL = 6 + AXIS_OFFSET + NEG_OFFSET, - DR = 6 + AXIS_OFFSET, + DR = 6 + AXIS_OFFSET + NEG_OFFSET, + DL = 6 + AXIS_OFFSET, DU = 7 + AXIS_OFFSET, DD = 7 + AXIS_OFFSET + NEG_OFFSET, LX = 0 + AXIS_OFFSET, @@ -45,8 +45,8 @@ namespace obelisk { Y1 = static_cast(Y) + LAYER_OFFSET, A1 = static_cast(A) + LAYER_OFFSET, B1 = static_cast(B) + LAYER_OFFSET, - DL1 = static_cast(DL) + LAYER_OFFSET, - DR1 = static_cast(DR) + LAYER_OFFSET, + DR1 = static_cast(DL) + LAYER_OFFSET, + DL1 = static_cast(DR) + LAYER_OFFSET, DU1 = static_cast(DU) + LAYER_OFFSET, DD1 = static_cast(DD) + LAYER_OFFSET, }; diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 3dfcb097..0b8d9551 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -46,12 +46,13 @@ namespace obelisk { } } - // Setup local variables + // Setup local variables (these will have UnitreeInterface ordering) joint_pos_.resize(G1_27DOF + G1_EXTRA_WAIST); joint_vel_.resize(G1_27DOF + G1_EXTRA_WAIST); start_user_pose_.resize(G1_27DOF + G1_EXTRA_WAIST); user_pose_.resize(G1_27DOF + G1_EXTRA_WAIST); + // Get user pose this->declare_parameter>("user_pose", user_pose_); user_pose_ = this->get_parameter("user_pose").as_double_array(); @@ -98,7 +99,6 @@ namespace obelisk { lowstate_subscriber_->InitChannel(std::bind(&G1Interface::LowStateHandler, this, std::placeholders::_1), 10); } - // TODO: Adjust this function so that we can go to a user pose even when no low level control functions are being sent void ApplyControl(const unitree_control_msg& msg) override { // Only execute of in Low Level Control or Home modes if (exec_fsm_state_ != ExecFSMState::USER_CTRL && exec_fsm_state_ != ExecFSMState::USER_POSE) { @@ -157,8 +157,8 @@ namespace obelisk { } use_default_gains = false; } - - for (size_t msg_ind = 0; msg_ind < num_motors_; msg_ind++) { // Only go through the non-hand motors + // Write message + for (size_t msg_ind = 0; msg_ind < num_motors_; msg_ind++) { int uni_ind = G1_JOINT_MAPPINGS.at(msg.joint_names[msg_ind]); int def_ind = default_joint_mapping_.at(msg.joint_names[msg_ind]); dds_low_command.motor_cmd().at(uni_ind).mode() = 1; // 1:Enable, 0:Disable @@ -179,7 +179,7 @@ namespace obelisk { dds_low_command.motor_cmd().at(uni_ind).kd() = 0; } } - // ------------------------ Execution FSM: Home ------------------------ // + // ------------------------ Execution FSM: USER POSE ------------------------ // } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { // Compute time that robot has been in HOME float t = std::chrono::duration( @@ -214,13 +214,10 @@ namespace obelisk { // Write command to the robot dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command, (sizeof(dds_low_command) >> 2) - 1); lowcmd_publisher_->Write(dds_low_command); - - // TODO: Deal with the hands } void LowStateHandler(const void *message) { LowState_ low_state = *(const LowState_ *)message; - // TODO: Right now we always report the full state (minus the hands for now) regardless of the joint mode if (low_state.crc() != Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) { RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeRobotInterface] CRC Error"); return; @@ -298,7 +295,7 @@ namespace obelisk { } bool CheckDampingToUnitreeHomeTransition() { - // Unitree checks this + // Unitree checks this for us return true; } @@ -342,7 +339,7 @@ namespace obelisk { } else { // use low level damping LowCmd_ dds_low_command; - for (size_t j = 0; j < num_motors_; j++) { // Only go through the non-hand motors + for (size_t j = 0; j < num_motors_; j++) { int i = G1_JOINT_MAPPINGS.at(joint_names_[j]); dds_low_command.motor_cmd().at(i).mode() = 1; // 1:Enable, 0:Disable dds_low_command.motor_cmd().at(i).tau() = 0; @@ -369,7 +366,6 @@ namespace obelisk { } void TransitionToUnitreeVel() override{ - // TODO: does this need to be placed in a certain mode? return; } @@ -484,6 +480,63 @@ namespace obelisk { this->log_count_++; } + void VerifyParameters() override { + ObeliskUnitreeInterface::VerifyParameters(); + + // --- helpers --- + auto fail = [&](const std::string& msg) -> void { + RCLCPP_ERROR_STREAM(this->get_logger(), msg); + throw std::runtime_error(msg); + }; + + const auto build_expected = [&]() { + std::unordered_set s; + s.reserve(G1_27DOF_JOINT_NAMES.size() + + (fixed_waist_ ? 0 : G1_EXTRA_WAIST_JOINT_NAMES.size())); + s.insert(G1_27DOF_JOINT_NAMES.begin(), G1_27DOF_JOINT_NAMES.end()); + if (!fixed_waist_) { + s.insert(G1_EXTRA_WAIST_JOINT_NAMES.begin(), + G1_EXTRA_WAIST_JOINT_NAMES.end()); + } + return s; + }; + + const auto expected = build_expected(); + + const size_t N = default_joint_names_.size(); + if (N != expected.size()) { + fail("[UnitreeInterface] default_joint_names has size " + + std::to_string(N) + ", but expected " + + std::to_string(expected.size())); + } + + for (const auto& name : default_joint_names_) { + if (expected.find(name) == expected.end()) { + fail("[UnitreeInterface] Unknown joint in default_joint_names: " + name); + } + } + + // Build name -> index map + default_joint_mapping_.clear(); + default_joint_mapping_.reserve(N); + for (size_t i = 0; i < N; ++i) { + default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); + } + + // Size checks (collapse repetition) + auto check_size = [&](const auto& v, const char* label) { + if (v.size() != N) { + fail(std::string("[UnitreeInterface] ") + label + " has size " + + std::to_string(v.size()) + ", but expected " + std::to_string(N)); + } + }; + + check_size(kp_, "default kp"); + check_size(kd_, "default kd"); + check_size(kd_damping_, "default kd_damping_"); + check_size(user_pose_, "user_pose_"); + } + inline static const std::string ROBOT_NAME = "g1"; const std::string& RobotName() const override { return ROBOT_NAME; } @@ -631,79 +684,3 @@ namespace obelisk { }; }; } // namespace obelisk - - -// std::unordered_set expected; -// if (fixed_waist_) { -// expected.reserve(G1_27DOF_JOINT_NAMES.size()); -// for (const auto& name : G1_27DOF_JOINT_NAMES) { -// expected.insert(name); -// } -// } else { -// expected.reserve(G1_27DOF_JOINT_NAMES.size() + G1_EXTRA_WAIST_JOINT_NAMES.size()); -// for (const auto& name : G1_27DOF_JOINT_NAMES) { -// expected.insert(name); -// } -// for (const auto& name : G1_EXTRA_WAIST_JOINT_NAMES) { -// expected.insert(name); -// } -// } - -// if (default_joint_names_.size() != expected.size()) { -// RCLCPP_ERROR_STREAM(this->get_logger(), -// "[UnitreeInterface] default_joint_names has size " + -// std::to_string(default_joint_names_.size()) + -// ", but expected " + std::to_string(expected.size())); -// throw std::runtime_error( -// "[UnitreeInterface] default_joint_names has size " + -// std::to_string(default_joint_names_.size()) + -// ", but expected " + std::to_string(expected.size())); -// } -// for (const auto& name : default_joint_names_) { -// if (expected.count(name) == 0) { -// RCLCPP_ERROR_STREAM(this->get_logger(), "[UnitreeInterface] Unknown joint in default_joint_names: " + name); -// throw std::runtime_error( -// "[UnitreeInterface] Unknown joint in default_joint_names: " + name); -// } -// } -// default_joint_mapping_.clear(); -// for (size_t i = 0; i < default_joint_names_.size(); ++i) { -// default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); -// } - -// if (kp_.size() != default_joint_names_.size()) { -// RCLCPP_ERROR_STREAM(this->get_logger(), -// "[UnitreeInterface] default kp has size " + -// std::to_string(kp_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// throw std::runtime_error("[UnitreeInterface] default kp has size " + -// std::to_string(kp_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// } -// if (kd_.size() != default_joint_names_.size()) { -// RCLCPP_ERROR_STREAM(this->get_logger(), -// "[UnitreeInterface] default kd has size " + -// std::to_string(kd_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// throw std::runtime_error("[UnitreeInterface] default kd has size " + -// std::to_string(kd_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// } -// if (kd_damping_.size() != default_joint_names_.size()) { -// RCLCPP_ERROR_STREAM(this->get_logger(), -// "[UnitreeInterface] default kd_damping_ has size " + -// std::to_string(kd_damping_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// throw std::runtime_error("[UnitreeInterface] default kd_damping_ has size " + -// std::to_string(kd_damping_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// } -// if (user_pose_.size() != default_joint_names_.size()) { -// RCLCPP_ERROR_STREAM(this->get_logger(), -// "[UnitreeInterface] user_pose_ has size " + -// std::to_string(user_pose_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// throw std::runtime_error("[UnitreeInterface] user_pose_ has size " + -// std::to_string(user_pose_.size()) + -// ", but expected " + std::to_string(default_joint_names_.size())); -// } \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index 7d53a1d7..b6d9658e 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -40,7 +40,7 @@ onboard: callback_group: None # ----- Joystick Passthrough ----- # - ros_parameter: pub_joy_passthrough_setting - topic: /obelisk/fsm_test/joy_passthrough + topic: /obelisk/g1/joy_passthrough history_depth: 10 callback_group: None subscribers: From 5dac6907e76f3731550e9080d49e25aecd345438 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 11 Nov 2025 23:41:56 -0800 Subject: [PATCH 27/27] fixed verifyParameters --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 0b8d9551..62f34ad5 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -480,7 +480,7 @@ namespace obelisk { this->log_count_++; } - void VerifyParameters() override { + void VerifyParameters() { ObeliskUnitreeInterface::VerifyParameters(); // --- helpers --- @@ -518,7 +518,6 @@ namespace obelisk { // Build name -> index map default_joint_mapping_.clear(); - default_joint_mapping_.reserve(N); for (size_t i = 0; i < N; ++i) { default_joint_mapping_.emplace(default_joint_names_[i], static_cast(i)); }