Fsm refactor

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

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.

obelisk/cpp/hardware/include/unitree_example_controller.h

@@ -1,9 +1,11 @@
+#include <map>
 #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<unitree_controller_msg, unitree_estimator_msg>(name), joint_idx_(0) {
 
             this->declare_parameter<std::string>("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<sensor_msgs::msg::Joy>(
@@ -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<std::string> joint_names_;
+        std::map<std::string, float> joint_defaults_;
+        std::string robot_str_;
 
         static constexpr int G1_MOTOR_NUM = 27;
         const std::vector<std::string> G1_JOINT_NAMES = {
@@ -182,5 +181,52 @@ namespace obelisk {
             "RL_thigh_joint",
             "RL_calf_joint"
         };
+
+        const std::map<std::string, float> 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<std::string, float> 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

obelisk/cpp/hardware/include/unitree_fsm.h

@@ -0,0 +1,24 @@
+#pragma once
+#include <cstdint>
+
+
+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<ExecFSMState, std::string> 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"}
+};