diff --git a/docs/source/images/UnitreeFSM.png b/docs/source/images/UnitreeFSM.png new file mode 100644 index 00000000..dd03cc7e Binary files /dev/null and b/docs/source/images/UnitreeFSM.png differ 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..4a68a107 --- /dev/null +++ b/docs/source/unitree_interfaces.md @@ -0,0 +1,100 @@ +# 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. 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): + +``` +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 // DL1 + user_pose_button: 127 // DR1 + 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, +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, +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, +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. + + +## 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_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 new file mode 100644 index 00000000..f6bd1947 --- /dev/null +++ b/obelisk/cpp/hardware/include/unitree_fsm.h @@ -0,0 +1,24 @@ +#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 +}; + + +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 6897e24e..8d441020 100644 --- a/obelisk/cpp/hardware/include/unitree_joystick.h +++ b/obelisk/cpp/hardware/include/unitree_joystick.h @@ -1,125 +1,437 @@ +#include +#include +#include +#include +#include + #include "obelisk_controller.h" #include "obelisk_ros_utils.h" #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 { 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: + 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, + DR = 6 + AXIS_OFFSET + NEG_OFFSET, + DL = 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, + DR1 = static_cast(DL) + LAYER_OFFSET, + DL1 = static_cast(DR) + LAYER_OFFSET, + DU1 = static_cast(DU) + LAYER_OFFSET, + DD1 = static_cast(DD) + LAYER_OFFSET, + }; + 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(); + 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 - 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("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_); + unitree_home_button_on_axis_ = isOnAxis(unitree_home_button_); + 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", 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 (!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", 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 (!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", 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 (!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", static_cast(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 (!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", 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 (!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", 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 (!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", 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 (!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", 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 (!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", 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::unordered_set s(values.begin(), values.end()); + 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_); + this->RegisterObkPublisher("pub_joy_passthrough_setting", pub_joy_passthrough_key_); RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick node has been initialized."); } + + inline static const std::unordered_map BUTTON_NAMES = { + {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_; - - this->GetPublisher(this->ctrl_key_)->publish(high_level_msg); - - // ----- Axes ----- // - constexpr int DPAD_VERTICAL = 7; - constexpr int DPAD_HORIZONTAL = 6; - constexpr int MENU = 7; - - // Print control menu + // 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() > 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 = static_cast(ExecFSMState::UNITREE_HOME); // Unitree Stand + commanded = true; + } else if (getUserCtrlButton(msg)) { + 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 = static_cast(ExecFSMState::UNITREE_VEL_CTRL); // Unitree Control + commanded = true; + } + if (commanded) { + 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 " << 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[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: " << ((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: " << 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: Left Stick Vertical\n" << - "\tSide/Side: Left Stick Horizontal\n" << - "\tYaw Rate: Right Stick Horizontal" + "\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 - static rclcpp::Time last_Dpad_press = this->now(); - if ((this->now() - last_Dpad_press).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 { + // 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; } - last_Dpad_press = this->now(); - this->GetPublisher(pub_exec_fsm_key_)->publish(fsm_msg); - RCLCPP_INFO_STREAM(this->get_logger(), "UnitreeJoystick sent Execution FSM Command."); + + 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) { + return BUTTON_NAMES.count(static_cast(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_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, 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 getAxisValue(msg, btn - AXIS_OFFSET) < axis_threshold_; } + if (on_dpad) { + int sgn = 1; + 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.axes[btn - AXIS_OFFSET] * sgn > 0.5; + } + return msg.buttons[btn]; + } + + float getAxisValue(const sensor_msgs::msg::Joy& msg, int ax) { + return msg.axes[ax]; } - unitree_high_level_ctrl_msg ComputeControl() override { + 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(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(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 { // 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; }; // 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_max_; - float v_y_max_; - float w_z_max_; + float v_x_scale_; + float v_y_scale_; + float w_z_scale_; + float axis_threshold_; // Hold button locations for execution fsm int damping_button_; @@ -127,11 +439,45 @@ 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 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_; + 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 diff --git a/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt b/obelisk/cpp/hardware/robots/unitree/CMakeLists.txt index 204aed9d..f614f2fa 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") @@ -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/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 8774c315..62f34ad5 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -1,17 +1,16 @@ #include "unitree_interface.h" -// Locomotion Client +// Unitree Clients #include #include +#include // IDL -#include #include +#include #include -#include #include -#include #include #include @@ -20,23 +19,20 @@ 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"), 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(); + // 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("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; } @@ -50,40 +46,42 @@ namespace obelisk { } } - // Setup local variables - // TODO: For now we are not supporting the hands here + // 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(); + + // Handle default joint 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(); + 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); + 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 - - // 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(); CreateUnitreePublishers(); - // Initialize logging files if logging is enabled - if (logging_) { - startup_time_ = this->now(); - 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 { @@ -99,18 +97,11 @@ 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); - - // 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); } - // 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 +112,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()); @@ -166,43 +157,55 @@ 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]; + // 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 + 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 ------------------------ // + // ------------------------ 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( - // 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 - 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).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 < 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.; + 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() = (1 - proportion) * 10 + proportion * 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!"); @@ -211,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; @@ -226,17 +226,28 @@ 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) { - 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"; + { + 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_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_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(); + + ind++; } this->GetPublisher(pub_joint_state_key_)->publish(joint_state); @@ -260,8 +271,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 @@ -271,138 +283,182 @@ 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 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); + 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 for us + return true; } 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_.BalanceStand(); + loco_client_.StopMove(); + 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 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_; - + void TransitionToDamp() override{ + if (high_level_ctrl_engaged_) { + // use high level damping + loco_client_.StopMove(); + loco_client_.Damp(); + } else { + // use low level damping + LowCmd_ dds_low_command; + 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; + 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); + } + + } - enum class AnkleMode { - PR = 0, // Series Control for Ptich/Roll Joints - AB = 1 // Parallel Control for A/B Joints - }; + void TransitionToUnitreeVel() override{ + return; + } - AnkleMode mode_pr_; - uint8_t mode_machine_; + 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_->SelectMode("normal") == 0; + } else { + ret_b = true; + } + if (ret_b) { + 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; + } - // Locomotion Client - g1::LocoClient loco_client_; + 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; + } - // Logging - std::ofstream motor_data_log_; - rclcpp::Time startup_time_; + bool ret_b; + if (!motion_mode.empty()) { + ret_b = motion_switcher_client_->ReleaseMode() == 0; + } else { + ret_b = true; + } + if (ret_b) { + high_level_ctrl_engaged_ = false; + } + return ret_b; + } - 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"; 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(); - - 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); 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); } } - 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(); @@ -413,7 +469,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() @@ -424,6 +480,79 @@ namespace obelisk { this->log_count_++; } + void VerifyParameters() { + 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(); + 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; } + + bool use_hands_; + bool fixed_waist_; + + ChannelSubscriberPtr lowstate_subscriber_; + unitree::robot::ChannelPublisherPtr lowcmd_publisher_; + + 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_; + private: // ---------- Robot Specific ---------- // // G1 @@ -431,30 +560,30 @@ 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 - // 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::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 default_joint_names_; + 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 + 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, // 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", @@ -484,20 +613,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 = { @@ -535,61 +650,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" - }; - - // 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}, @@ -622,4 +682,4 @@ namespace obelisk { {"right_wrist_yaw_joint", 28} }; }; -} // namespace obelisk \ No newline at end of file +} // namespace obelisk diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 3a107ded..40b6a515 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 @@ -13,27 +12,24 @@ 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) { - - // Additional Publishers - this->RegisterObkPublisher("pub_force_sensor_setting", "force_sensor_key"); + : ObeliskUnitreeInterface(node_name, "go2") { // 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(); - // 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 + 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 +39,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 +64,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 +77,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 +179,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(); } @@ -217,36 +212,21 @@ namespace obelisk { 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]; + if (logging_) { + std::lock_guard lk(motor_state_mtx_); + motor_state_ = low_state.motor_state(); } - 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; - } + 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); } } @@ -267,64 +247,161 @@ 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 } 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 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; - // // } + void TransitionToDamp() override{ + 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); + } + } - // // 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]; + void TransitionToUnitreeVel() override{ + sport_client_.ClassicWalk(false); + } - // 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]; + 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) { + 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; + } - // 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]; + 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 + 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{ + 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->GetPublisher("odom_state_pub")->publish(obk_imu_state); - // } + this->log_count_++; + } - std::string ODOM_TOPIC_; + 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_; - // 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 vel_deadzone_; // Deadzone for high level velocity commands + 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 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..9c8e029e 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" @@ -10,19 +11,17 @@ #include #include #include - +#include // DDS #include #include -// IDL -#include 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; @@ -30,36 +29,67 @@ 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 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 exec_fsm_state_ = ExecFSMState::INIT; + 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_); 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"); + 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 + 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); 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); @@ -67,29 +97,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_high_level_ctrl_setting", sub_high_level_ctrl_key_, std::bind(&ObeliskUnitreeInterface::ApplyHighLevelControl, 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( @@ -98,58 +112,59 @@ namespace obelisk { CreateUnitreeSubscribers(); + if (!high_level_ctrl_engaged_) { + high_level_ctrl_engaged_ = true; // Forcing Unitree state machine to transition (see ReleaseUnitreeMotionControl) + ReleaseUnitreeMotionControl(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - enum class ExecFSMState : uint8_t { - INIT = 0, - UNITREE_HOME = 1, - USER_POSE = 2, - LOW_LEVEL_CTRL = 3, - HIGH_LEVEL_CTRL = 4, - DAMPING = 5 - }; - - 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"} - }; - - // 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::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) }; // 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 }; // 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 void TransitionToUnitreeVel() = 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 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)) { @@ -160,34 +175,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(); + 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 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 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(); + 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 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 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 (cmd_exec_fsm_state == ExecFSMState::DAMPING) { + TransitionToDamp(); + } else if (cmd_exec_fsm_state == ExecFSMState::UNITREE_HOME) { TransitionToUnitreeHome(); - } else if (exec_fsm_state_ == ExecFSMState::USER_POSE) { + } else if (cmd_exec_fsm_state == ExecFSMState::UNITREE_VEL_CTRL) { + TransitionToUnitreeVel(); + } 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_)); @@ -250,71 +266,88 @@ 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."); + 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(), "[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; + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open motor data log file: " << motor_data_file); } - return true; + + WriteJointNameFile(); } // Execution FSM state variable ExecFSMState exec_fsm_state_; + bool high_level_ctrl_engaged_; // ---------- Topics ---------- // - // TODO: Verify these are the same of the G1 and the Go2 std::string CMD_TOPIC_; std::string STATE_TOPIC_; std::string network_interface_name_; - std::shared_ptr mode_switch_manager_; 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_; + std::vector kd_damping_; // 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"; + const std::string timer_logging_key_ = "timer_logging_key"; // Logging bool logging_; std::string log_dir_path_; long log_count_; + 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/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/python/obelisk_py/core/utils/launch_utils.py b/obelisk/python/obelisk_py/core/utils/launch_utils.py index 2bb8e818..a6739984 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( @@ -347,27 +357,24 @@ 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 == "on": + continue + 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 +383,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/g1_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml index bac0fdb9..b6d9658e 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_cpp.yaml @@ -35,7 +35,12 @@ 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 + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/g1/joy_passthrough history_depth: 10 callback_group: None subscribers: @@ -44,7 +49,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 @@ -68,7 +73,7 @@ onboard: callback_group: None # sensing: robot: - # # === simulation === + # === simulation === # - is_simulated: True # pkg: obelisk_unitree_cpp # executable: obelisk_unitree_sim @@ -79,28 +84,61 @@ onboard: pkg: obelisk_unitree_cpp executable: obelisk_unitree_g1_hardware params: - network_interface_name: enp4s0 + # 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 - ] + 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: [ - 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 + 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 + 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., # 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: @@ -111,7 +149,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 @@ -129,8 +167,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: @@ -139,7 +177,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 @@ -161,8 +201,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 @@ -172,14 +210,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 @@ -188,13 +218,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 @@ -211,8 +234,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 @@ -222,14 +243,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 @@ -237,18 +250,13 @@ 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 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 31287381..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 @@ -98,6 +102,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: @@ -129,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: @@ -190,6 +200,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/unitree_fsm_debug.yaml b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml new file mode 100644 index 00000000..44bc36ee --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/unitree_fsm_debug.yaml @@ -0,0 +1,35 @@ +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 + # ----- 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 + 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"], 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 @@ - +