feat: support G1 23dof (U1version) robot type

deploy/robots/g1_23dof/CMakeLists.txt

@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.12)
 project(g1_29dof_controller)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS ON)
+
 find_package(Boost REQUIRED COMPONENTS program_options)
 find_package(yaml-cpp REQUIRED)
 

deploy/robots/g1_23dof/config/config.yaml

@@ -7,6 +7,14 @@ FSM:
     Velocity:
       id: 3
       type: RLBase
+
+    Mimic_Dance_102:
+      id: 101
+      type: Mimic
+    Mimic_Gangnam_Style:
+      id: 102
+      type: Mimic
+
   Passive:
     transitions: 
       FixStand: LT + up.on_pressed
@@ -55,4 +63,26 @@ FSM:
   Velocity:
     transitions: 
       Passive: LT + B.on_pressed
-    policy_dir: ../../../logs/rsl_rl/unitree_g1_23dof_velocity
+      Mimic_Dance_102: LT(2s) + down.on_pressed
+      Mimic_Gangnam_Style: LT(2s) + left.on_pressed
+      Mimic_Punch: LT(2s) + up.on_pressed
+    policy_dir: config/policy/velocity
+
+  Mimic_Dance_102:
+    transitions:
+      Passive: LT + B.on_pressed
+      Velocity: RB + X.on_pressed
+    fps: 60
+    motion_file: config/policy/mimic/dance_102/params/G1_Take_102.bvh_60hz_23dof.csv
+    policy_dir: config/policy/mimic/dance_102/
+    time_start: 2.8
+    time_end: 24.0
+
+  Mimic_Gangnam_Style:
+    transitions:
+      Passive: LT + B.on_pressed
+      Velocity: RB + X.on_pressed
+    fps: 60
+    motion_file: config/policy/mimic/gangnam_style/params/G1_gangnam_style_V01.bvh_60hz_23dof.csv
+    policy_dir: config/policy/mimic/gangnam_style/
+

deploy/robots/g1_23dof/config/policy/mimic/dance_102/params/deploy.yaml

@@ -0,0 +1,58 @@
+joint_ids_map: [0, 6, 12, 1, 7, 15, 22, 2, 8, 16, 23, 3, 9, 17, 24, 4, 10, 18, 25,
+  5, 11, 19, 26]
+step_dt: 0.02
+stiffness: [40.2, 99.1, 40.2, 99.1, 28.5, 28.5, 40.2, 99.1, 40.2, 99.1, 28.5, 28.5,
+  40.2, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3]
+damping: [2.56, 6.31, 2.56, 6.31, 1.81, 1.81, 2.56, 6.31, 2.56, 6.31, 1.81, 1.81,
+  2.56, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907]
+default_joint_pos: [-0.302, -0.319, 0.00124, 0.000442, 0.00489, 0.202, 0.209, 0.00796,
+  0.00546, 0.203, -0.199, 0.669, 0.671, -0.00489, 0.00793, -0.369, -0.359, 0.594,
+  0.61, 0.00258, -0.00029, 0.00475, -0.00448]
+commands: {}
+actions:
+  JointPositionAction:
+    clip: null
+    joint_names: [.*]
+    scale: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 0.439, 0.548, 0.548, 0.439,
+      0.439, 0.351, 0.351, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439,
+      0.439, 0.439]
+    offset: [-0.302, -0.319, 0.00124, 0.000442, 0.00489, 0.202, 0.209, 0.00796, 0.00546,
+      0.203, -0.199, 0.669, 0.671, -0.00489, 0.00793, -0.369, -0.359, 0.594, 0.61,
+      0.00258, -0.00029, 0.00475, -0.00448]
+    joint_ids: null
+observations:
+  motion_command:
+    params: {command_name: motion}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  motion_anchor_ori_b:
+    params: {command_name: motion}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  base_ang_vel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0]
+    history_length: 1
+  joint_pos_rel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  joint_vel_rel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  last_action:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1

deploy/robots/g1_23dof/config/policy/mimic/gangnam_style/params/deploy.yaml

@@ -0,0 +1,58 @@
+joint_ids_map: [0, 6, 12, 1, 7, 15, 22, 2, 8, 16, 23, 3, 9, 17, 24, 4, 10, 18, 25,
+  5, 11, 19, 26]
+step_dt: 0.02
+stiffness: [40.2, 99.1, 40.2, 99.1, 28.5, 28.5, 40.2, 99.1, 40.2, 99.1, 28.5, 28.5,
+  40.2, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3, 14.3]
+damping: [2.56, 6.31, 2.56, 6.31, 1.81, 1.81, 2.56, 6.31, 2.56, 6.31, 1.81, 1.81,
+  2.56, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907, 0.907]
+default_joint_pos: [-0.302, -0.319, 0.00124, 0.000442, 0.00489, 0.202, 0.209, 0.00796,
+  0.00546, 0.203, -0.199, 0.669, 0.671, -0.00489, 0.00793, -0.369, -0.359, 0.594,
+  0.61, 0.00258, -0.00029, 0.00475, -0.00448]
+commands: {}
+actions:
+  JointPositionAction:
+    clip: null
+    joint_names: [.*]
+    scale: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 0.439, 0.548, 0.548, 0.439,
+      0.439, 0.351, 0.351, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439,
+      0.439, 0.439]
+    offset: [-0.302, -0.319, 0.00124, 0.000442, 0.00489, 0.202, 0.209, 0.00796, 0.00546,
+      0.203, -0.199, 0.669, 0.671, -0.00489, 0.00793, -0.369, -0.359, 0.594, 0.61,
+      0.00258, -0.00029, 0.00475, -0.00448]
+    joint_ids: null
+observations:
+  motion_command:
+    params: {command_name: motion}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  motion_anchor_ori_b:
+    params: {command_name: motion}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  base_ang_vel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0]
+    history_length: 1
+  joint_pos_rel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  joint_vel_rel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1
+  last_action:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 1

deploy/robots/g1_23dof/config/policy/velocity/v0/params/deploy.yaml

@@ -0,0 +1,58 @@
+joint_ids_map: [0, 6, 12, 1, 7, 15, 22, 2, 8, 16, 23, 3, 9, 17, 24, 4, 10, 18, 25, 5, 11, 19, 26]
+step_dt: 0.02
+stiffness: [100.0, 100.0, 100.0, 150.0, 40.0, 40.0, 100.0, 100.0, 100.0, 150.0, 40.0,
+  40.0, 200.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0]
+damping: [2.0, 2.0, 2.0, 4.0, 2.0, 2.0, 2.0, 2.0, 2.0, 4.0, 2.0, 2.0, 5.0, 1.0, 1.0,
+  1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+default_joint_pos: [-0.1, -0.1, 0.0, 0.0, 0.0, 0.3, 0.3, 0.0, 0.0, 0.25, -0.25, 0.3,
+  0.3, 0.0, 0.0, -0.2, -0.2, 0.97, 0.97, 0.0, 0.0, 0.15, -0.15]
+commands:
+  base_velocity:
+    ranges:
+      lin_vel_x: [-0.5, 1.0]
+      lin_vel_y: [-0.3, 0.3]
+      ang_vel_z: [-1.2, 1.2]
+      heading: null
+actions:
+  JointPositionAction:
+    clip: null
+    joint_names: [.*]
+    scale: [0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25,
+      0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25, 0.25]
+    offset: [-0.1, -0.1, 0.0, 0.0, 0.0, 0.3, 0.3, 0.0, 0.0, 0.25, -0.25, 0.3, 0.3,
+      0.0, 0.0, -0.2, -0.2, 0.97, 0.97, 0.0, 0.0, 0.15, -0.15]
+    joint_ids: null
+observations:
+  base_ang_vel:
+    params: {}
+    clip: null
+    scale: [0.2, 0.2, 0.2]
+    history_length: 5
+  projected_gravity:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0]
+    history_length: 5
+  velocity_commands:
+    params: {command_name: base_velocity}
+    clip: null
+    scale: [1.0, 1.0, 1.0]
+    history_length: 5
+  joint_pos_rel:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 5
+  joint_vel_rel:
+    params: {}
+    clip: null
+    scale: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05,
+      0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
+    history_length: 5
+  last_action:
+    params: {}
+    clip: null
+    scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+      1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    history_length: 5

deploy/robots/g1_23dof/include/State_Mimic.h

@@ -0,0 +1,114 @@
+#pragma once
+
+#include "FSM/State_RLBase.h"
+
+class State_Mimic : public FSMState
+{
+public:
+    State_Mimic(int state_mode, std::string state_string);
+
+    void enter();
+
+    void run();
+
+    void exit()
+    {
+        policy_thread_running = false;
+        if (policy_thread.joinable()) {
+            policy_thread.join();
+        }
+    }
+
+    class MotionLoader_;
+
+    static std::shared_ptr<MotionLoader_> motion;
+private:
+    std::unique_ptr<isaaclab::ManagerBasedRLEnv> env;
+    std::shared_ptr<MotionLoader_> motion_;
+
+    std::thread policy_thread;
+    bool policy_thread_running = false;
+    std::array<float, 2> time_range_;
+};
+
+class State_Mimic::MotionLoader_
+{
+public:
+    MotionLoader_(std::string motion_file, float fps)
+    : dt(1.0f / fps)
+    {
+        auto data = isaaclab::load_csv(motion_file);
+
+        num_frames = data.size();
+        duration = num_frames * dt;
+
+        for(int i(0); i < num_frames; ++i)
+        {
+            root_positions.push_back(Eigen::VectorXf::Map(data[i].data(), 3));
+            root_quaternions.push_back(Eigen::Quaternionf(data[i][6],data[i][3], data[i][4], data[i][5]));
+            dof_positions.push_back(Eigen::VectorXf::Map(data[i].data() + 7, data[i].size() - 7));
+        }
+        dof_velocities = _comupte_raw_derivative(dof_positions);
+
+        update(0.0f);
+    }
+
+    void update(float time)
+    {
+        float phase = std::clamp(time / duration, 0.0f, 1.0f);
+        index_0_ = std::round(phase * (num_frames - 1));
+        index_1_ = std::min(index_0_ + 1, num_frames - 1);
+        blend_ = std::round((time - index_0_ * dt) / dt * 1e5f) / 1e5f;
+    }
+
+    void reset(const isaaclab::ArticulationData & data, float t = 0.0f)
+    {
+        update(t);
+        auto init_to_anchor = isaaclab::yawQuaternion(this->root_quaternion()).toRotationMatrix();
+        auto world_to_anchor = isaaclab::yawQuaternion(data.root_quat_w).toRotationMatrix();
+        world_to_init_ = world_to_anchor * init_to_anchor.transpose();
+    }
+
+    Eigen::VectorXf joint_pos() {
+        return dof_positions[index_0_] * (1 - blend_) + dof_positions[index_1_] * blend_;
+    }
+
+    Eigen::VectorXf root_position() {
+        return root_positions[index_0_] * (1 - blend_) + root_positions[index_1_] * blend_;
+    }
+
+    Eigen::VectorXf joint_vel() {
+        return dof_velocities[index_0_] * (1 - blend_) + dof_velocities[index_1_] * blend_;
+    }
+
+    Eigen::Quaternionf root_quaternion() {
+        return root_quaternions[index_0_].slerp(blend_, root_quaternions[index_1_]);
+    }
+
+    float dt;
+    int num_frames;
+    float duration;
+
+    std::vector<Eigen::VectorXf> root_positions;
+    std::vector<Eigen::Quaternionf> root_quaternions;
+    std::vector<Eigen::VectorXf> dof_positions;
+    std::vector<Eigen::VectorXf> dof_velocities;
+
+    Eigen::Matrix3f world_to_init_;
+private:
+    int index_0_;
+    int index_1_;
+    float blend_;
+
+    std::vector<Eigen::VectorXf> _comupte_raw_derivative(const std::vector<Eigen::VectorXf>& data)
+    {
+        std::vector<Eigen::VectorXf> derivative;
+        for(int i = 0; i < data.size() - 1; ++i) {
+            derivative.push_back((data[i + 1] - data[i]) / dt);
+        }
+        derivative.push_back(derivative.back());
+        return derivative;
+    }
+};
+
+REGISTER_FSM(State_Mimic)

deploy/robots/g1_23dof/main.cpp

@@ -2,10 +2,11 @@
 #include "FSM/State_Passive.h"
 #include "FSM/State_FixStand.h"
 #include "FSM/State_RLBase.h"
+#include "State_Mimic.h"
 
 std::unique_ptr<LowCmd_t> FSMState::lowcmd = nullptr;
 std::shared_ptr<LowState_t> FSMState::lowstate = nullptr;
-std::shared_ptr<Keyboard> FSMState::keyboard = nullptr;
+std::shared_ptr<Keyboard> FSMState::keyboard = std::make_shared<Keyboard>();
 
 void init_fsm_state()
 {
@@ -49,6 +50,11 @@ int main(int argc, char** argv)
 
     std::cout << "Press [L2 + Up] to enter FixStand mode.\n";
     std::cout << "And then press [R1 + X] to start controlling the robot.\n";
+    std::cout << "Passive(Damping): L2 + B\n";
+    std::cout << "Velocity(Walking): R1 + X\n";
+    std::cout << "Mimic_Dance_102: L2(2s) + Down\n";
+    std::cout << "Mimic_Gangnam_Style: L2(2s) + Left\n";
+    std::cout << "Mimic_Punch: L2(2s) + Up\n";
 
     while (true)
     {