First Commit

Deploy_Tienkung/policy/beyond_mimic/config/BeyondMimic.yaml

@@ -0,0 +1,117 @@
+# onnx_path: "policy_fightAndSports1_s1.onnx"
+# onnx_path: "policy-dance.onnx"
+onnx_path: "lastdance_onlycom.onnx"
+warm_start_time: 0.0
+
+body_names : [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "elbow_pitch_r_link",
+]
+anchor_body : "pelvis"
+
+#without motion anchor pos && anchor base lin vel
+# command :23+23+3+6  55
+# motion_anchor_ori_b: 6
+# base_ang_vel: 3
+# joint_pos（相对轴角）: 23
+# joint_vel: 23
+# actions（上一时刻动作）: 23
+# total : 133
+num_obs : 133 
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 18, 
+ 22, 25 
+]
+
+kps: [
+        300, 300, 150, 350, 30, 16.8,
+        300, 300, 150, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,150,200,200,
+        150, 50, 50, 150,150,200,200,
+]
+kds: [
+        10, 10, 5, 10, 5.0, 2.0,
+        10, 10, 5, 10, 5.0, 2.0,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+# motion_length: 140 
+
+# tau_limit: [88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                   139.0, 139.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                                       25.0, 25.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0
+#            ]
+
+# kp_lab: [40.179, 40.179, 40.179, 99.098, 99.098, 28.501, 
+#           40.179, 40.179, 28.501, 99.098, 99.098, 14.251, 
+#           14.251, 28.501, 28.501, 14.251, 14.251, 28.501, 
+#           28.501, 14.251, 14.251, 14.251, 14.251, 14.251, 
+#           14.251, 16.778, 16.778, 16.778, 16.778]
+
+# kd_lab: [2.558, 2.558, 2.558, 6.309, 6.309, 1.814, 
+#           2.558, 2.558, 1.814, 6.309, 6.309, 0.907, 
+#           0.907, 1.814, 1.814, 0.907, 0.907, 1.814, 
+#           1.814, 0.907, 0.907, 0.907, 0.907, 0.907, 
+#           0.907, 1.068, 1.068, 1.068, 1.068]
+
+# default_angles_lab: [-0.312, -0.312, 0.0, 0.0, 0.0, 0.0, 
+#                     0.0, 0.0, 0.0, 0.669, 0.669, 0.2, 
+#                     0.2, -0.363, -0.363, 0.2, -0.2, 0.0, 
+#                     0.0, 0.0, 0.0, 0.6, 0.6, 0.0, 
+#                     0.0, 0.0, 0.0, 0.0, 0.0]
+
+# action_scale_lab: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 
+#                   0.548, 0.548, 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, 0.439, 0.439, 0.439, 
+#                   0.439, 0.075, 0.075, 0.075, 0.075]
+
+num_actions: 19
+motor_nums: 29
+# num_obs: 154
+hold_final_reference: false
+
+# mj2lab: [0, 6, 12,
+#                   1, 7, 13,
+#                   2, 8, 14,
+#                   3, 9    , 15, 22,
+#                   4, 10   , 16, 23,
+#                   5, 11   , 17, 24,
+#                             18, 25,
+#                             19, 26,
+#                             20, 27,
+#                             21, 28]
+# history_length: 4
+physical_dt: 0.01
+decimation: 1

Deploy_Tienkung/policy/beyond_mimic/fsm_beyond_mimic.py

@@ -0,0 +1,547 @@
+from FSM.fsm_base import FSMState, FSMStateName
+import numpy as np
+import yaml
+import os
+from types import SimpleNamespace
+from typing import Optional
+try:
+    import onnx
+except ImportError:  # pragma: no cover - runtime fallback when onnx isn't installed
+    onnx = None
+import onnxruntime
+try:
+    import torch
+except ImportError:  # pragma: no cover - torch is optional for warm start prep
+    torch = None
+from common.robot_data import RobotData
+from common.joystick import ControlFlag
+import time
+
+
+DEFAULT_BODY_NAMES = [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "shoulder_roll_l_link",
+    "shoulder_yaw_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "shoulder_roll_r_link",
+    "shoulder_yaw_r_link",
+    "elbow_pitch_r_link",
+]
+
+class FSMStateBeyondMimic(FSMState):
+    def __init__(self, robot_data: RobotData, config_path: Optional[str] = None, variant_name: str = "default"):
+        super().__init__(robot_data)
+        self.motion_phase = 0
+        self.counter_step = 0
+        self.ref_motion_phase = 0
+        self.variant_name = variant_name
+        
+        
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "BeyondMimic.yaml")
+        self.config_path = os.path.abspath(config_path)
+        with open(self.config_path, "r") as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+            # 兼容多策略：模型路径仍然默认指向 policy/beyond_mimic/model 下
+            self.onnx_path = config["onnx_path"]
+            if not os.path.isabs(self.onnx_path):
+                self.onnx_path = os.path.join(current_dir, "model", self.onnx_path)
+            # self.motion_length = config["motion_length"]
+            # self.history_length = config["history_length"]
+            self.physical_dt = config["physical_dt"]
+            self.decimation_ = config["decimation"]
+            self.num_actions = config["num_actions"]
+            self.motor_nums = config["motor_nums"]
+            self.warm_start_time = config["warm_start_time"]
+            self.kps = config["kps"]
+            self.kds = config["kds"]
+            self.hold_final_reference = config.get("hold_final_reference", False)
+            self.motion_length: Optional[int] = config.get("motion_length")
+            self.body_names = config.get("body_names", DEFAULT_BODY_NAMES)
+            self.locked_joint_map = config["locked_joint_map"]
+            self.anchor_body_name = config.get("anchor_body", "pelvis")
+            if self.anchor_body_name not in self.body_names:
+                raise ValueError(f"Anchor body {self.anchor_body_name} missing from body list.")
+            self.anchor_body_index = self.body_names.index(self.anchor_body_name)
+            self.num_bodies = len(self.body_names)
+            self._warm_start_from_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_to_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_prev_target = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.last_run_time = time.perf_counter()
+            
+            self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)  # 初始化为最大可能大小
+            self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+            self.num_obs = None  # set after loading onnx
+            self.obs = None
+            self.action = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.ref_joint_pos = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_joint_vel = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_body_pos_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_quat_w = np.zeros((1, self.num_bodies, 4), dtype=np.float32)
+            self.ref_body_lin_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_ang_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            # load policy
+            self.onnx_model = None
+            metadata_props = []
+            if onnx is not None and hasattr(onnx, "load"):
+                try:
+                    self.onnx_model = onnx.load(self.onnx_path)
+                    metadata_props = getattr(self.onnx_model, "metadata_props", [])
+                except Exception as exc:
+                    print(f"[FSMStateBeyondMimic] Failed to load ONNX model via onnx.load: {exc}")
+            else:
+                print("[FSMStateBeyondMimic] Python onnx package unavailable, falling back to onnxruntime metadata.")
+
+            self.ort_session = onnxruntime.InferenceSession(self.onnx_path)
+            if not metadata_props:
+                model_meta = self.ort_session.get_modelmeta()
+                custom_map = getattr(model_meta, "custom_metadata_map", {})
+                metadata_props = [SimpleNamespace(key=k, value=v) for k, v in custom_map.items()]
+
+            input = self.ort_session.get_inputs()
+            self.input_name = []
+            for i, inpt in enumerate(input):
+                self.input_name.append(inpt.name)
+            obs_input = self.ort_session.get_inputs()[0]
+            last_dim = obs_input.shape[-1]
+            if isinstance(last_dim, int):
+                self.num_obs = last_dim
+            else:
+                self.num_obs = config.get("num_obs", 154)
+            self.obs = np.zeros(self.num_obs, dtype=np.float32)
+
+            # 从ONNX模型中读取参数
+            self.joint_seq = None
+            self.joint_pos_array_seq = None
+            self.action_scale = None
+            # self.stiffness_array_seq = None
+            # self.damping_array_seq = None
+            
+            for prop in metadata_props:
+                if prop.key == "joint_names":
+                    self.joint_seq = prop.value.split(",")
+                if prop.key == "default_joint_pos":   
+                    self.joint_pos_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_stiffness":
+                #     self.stiffness_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_damping":
+                #     self.damping_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key == "action_scale":
+                    self.action_scale = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key in ("motion_length", "time_step_total"):
+                    try:
+                        self.motion_length = int(float(prop.value))
+                    except (TypeError, ValueError):
+                        print(f"[FSMStateBeyondMimic] Invalid motion_length metadata value: {prop.value}")
+
+            if self.motion_length is not None:
+                try:
+                    self.motion_length = int(self.motion_length)
+                except (TypeError, ValueError):
+                    print(f"[FSMStateBeyondMimic] Invalid motion_length config value: {self.motion_length}")
+                    self.motion_length = None
+            
+            # # 根据YAML配置设置关节映射
+            # self.mj2lab = np.array(config["mj2lab"], dtype=np.int32)
+            
+            # 设置从序列到实验室顺序的映射
+            self.joint_xml = [
+                "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+                "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+                "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+                "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+                "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+                "shoulder_pitch_l_joint",
+                "elbow_pitch_l_joint",
+                "shoulder_pitch_r_joint",
+                "elbow_pitch_r_joint", 
+            ]
+            # 从MjXUML顺序映射到实验室顺序
+            self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+
+            # 从实验室顺序映射到MjXUML顺序
+            self.joint_pos_array = np.array([self.joint_pos_array_seq[self.joint_seq.index(joint)] for joint in self.joint_xml])
+            self.default_angles_lab = self.joint_pos_array_seq
+            self.action_scale_lab = self.action_scale
+
+            print("BeyondMimic-like policy initializing ...")
+            self._warmup_inference_counter = 0
+            self.warm_start_steps = 0
+            # Cache for the last motion frame so we can keep sending it after motion ends.
+            self._final_ref_cached = False
+            self._final_ref_joint_pos = np.zeros_like(self.ref_joint_pos)
+            self._final_ref_joint_vel = np.zeros_like(self.ref_joint_vel)
+            self._final_ref_body_pos_w = np.zeros_like(self.ref_body_pos_w)
+            self._final_ref_body_quat_w = np.zeros_like(self.ref_body_quat_w)
+            self._final_ref_body_lin_vel_w = np.zeros_like(self.ref_body_lin_vel_w)
+            self._final_ref_body_ang_vel_w = np.zeros_like(self.ref_body_ang_vel_w)
+    
+    def on_enter(self):
+        self.ref_motion_phase = 0.
+        self.motion_time = 0
+        self.counter_step = 0
+        self._warmup_inference_counter = 0
+        print(f"[FSMStateBeyondMimic] enter variant={self.variant_name}, config={self.config_path}")
+        if self.warm_start_time > 0:
+            step = self.decimation_ * self.physical_dt
+            self.warm_start_steps = max(1, int(self.warm_start_time / step))
+        else:
+            self.warm_start_steps = 0
+
+        observation = {}
+        observation[self.input_name[0]] = np.zeros((1, self.num_obs), dtype=np.float32)
+        observation[self.input_name[1]] = np.zeros((1, 1), dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.obs = np.zeros(self.num_obs)
+        self._final_ref_cached = False
+        self._warm_start_from_lab = self._get_current_joint_pos_lab()
+        self._warm_start_to_lab = self._get_onnx_first_frame_lab()
+        self._warm_start_prev_target = np.array(self._warm_start_from_lab, copy=True)
+
+        # self.action = np.zeros(self.num_actions)
+
+        pass
+        
+    def quat_mul(self, q1, q2):
+        w1, x1, y1, z1 = q1[0], q1[1], q1[2], q1[3]
+        w2, x2, y2, z2 = q2[0], q2[1], q2[2], q2[3]
+        # perform multiplication
+        ww = (z1 + x1) * (x2 + y2)
+        yy = (w1 - y1) * (w2 + z2)
+        zz = (w1 + y1) * (w2 - z2)
+        xx = ww + yy + zz
+        qq = 0.5 * (xx + (z1 - x1) * (x2 - y2))
+        w = qq - ww + (z1 - y1) * (y2 - z2)
+        x = qq - xx + (x1 + w1) * (x2 + w2)
+        y = qq - yy + (w1 - x1) * (y2 + z2)
+        z = qq - zz + (z1 + y1) * (w2 - x2)
+        return np.array([w, x, y, z])
+        
+    def matrix_from_quat(self, q):
+        w, x, y, z = q
+        return np.array([
+            [1 - 2 * (y**2 + z**2), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x**2 + z**2), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x**2 + y**2)]
+        ])
+
+    def yaw_quat(self, q):
+        w, x, y, z = q
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
+        return np.array([np.cos(yaw / 2), 0, 0, np.sin(yaw / 2)])
+    
+    def euler_single_axis_to_quat(self, angle, axis, degrees=False):
+        """
+        将单个欧拉角转换为四元数
+        
+        参数:
+            angle: 旋转角度
+            axis: 旋转轴，可以是 'x', 'y', 'z' 或者单位向量 [x, y, z]
+            degrees: 如果为True，输入角度为度数；如果为False，输入角度为弧度
+        
+        返回:
+            四元数 (w, x, y, z)
+        """
+        # 转换角度为弧度
+        if degrees:
+            angle = np.radians(angle)
+        
+        # 计算半角
+        half_angle = angle * 0.5
+        cos_half = np.cos(half_angle)
+        sin_half = np.sin(half_angle)
+        
+        # 根据旋转轴确定四元数分量
+        if isinstance(axis, str):
+            if axis.lower() == 'x':
+                return np.array([cos_half, sin_half, 0.0, 0.0])
+            elif axis.lower() == 'y':
+                return np.array([cos_half, 0.0, sin_half, 0.0])
+            elif axis.lower() == 'z':
+                return np.array([cos_half, 0.0, 0.0, sin_half])
+            else:
+                raise ValueError("axis must be 'x', 'y', 'z' or a 3D unit vector")
+        else:
+            # 假设axis是一个3D向量 [x, y, z]
+            axis = np.array(axis, dtype=np.float32)
+            # 归一化轴向量
+            axis_norm = np.linalg.norm(axis)
+            if axis_norm == 0:
+                raise ValueError("axis vector cannot be zero")
+            axis = axis / axis_norm
+            
+            # 计算四元数分量
+            w = cos_half
+            x = sin_half * axis[0]
+            y = sin_half * axis[1]
+            z = sin_half * axis[2]
+            
+            return np.array([w, x, y, z])
+
+    def inner_run(self):
+        robot_quat = self.robot_data_.get_robot_quat()
+        qj = self.robot_data_.get_joint_pos()
+        # 将29dof自由度的数据映射回锁住腕部6关节，之后的逻辑和和之前没区别
+        qj = qj[self.locked_joint_map]
+
+        qj = qj[self.mj2lab]
+        qj = (qj - self.default_angles_lab)
+
+        # IMU mounted on pelvis, so directly use measured orientation.
+        ref_anchor_pos_w, ref_anchor_ori_w = self._get_ref_anchor_pose()
+
+        # 在第一帧提取当前机器人yaw方向，与参考动作yaw方向做差（与beyond mimic一致）
+        if(self.counter_step < 2):
+            init_to_anchor = self.matrix_from_quat(self.yaw_quat(ref_anchor_ori_w))
+            world_to_anchor = self.matrix_from_quat(self.yaw_quat(robot_quat))
+            self.init_to_world = world_to_anchor @ init_to_anchor.T
+            print("self.init_to_world: ", self.init_to_world)
+            self.counter_step += 1
+            return
+
+        robot_rot_mat = self.matrix_from_quat(robot_quat)
+        motion_anchor_ori_b = robot_rot_mat.T @ self.init_to_world @ self.matrix_from_quat(ref_anchor_ori_w)
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+
+        dqj = self.robot_data_.get_joint_vel()
+        #映射到23dof
+        dqj = dqj[self.locked_joint_map]
+
+        use_warmstart = (
+            self.warm_start_steps > 0
+            and self._warmup_inference_counter < self.warm_start_steps
+        )
+        blended_target = None
+        if use_warmstart:
+            blend = (self._warmup_inference_counter + 1) / self.warm_start_steps
+            blended_target = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+            blended_vel = (blended_target - self._warm_start_prev_target) / (
+                self.decimation_ * self.physical_dt
+            )
+            self._warm_start_prev_target = blended_target
+
+            command_joint_pos = blended_target.reshape(1, -1)
+            command_joint_vel = blended_vel.reshape(1, -1)
+            safe_scale = np.where(self.action_scale_lab == 0, 1.0, self.action_scale_lab)
+            action_for_history = (blended_target - self.default_angles_lab) / safe_scale
+        else:
+            command_joint_pos = self.ref_joint_pos
+            command_joint_vel = self.ref_joint_vel
+            action_for_history = self.action
+
+        command_root = self.matrix_from_quat(ref_anchor_ori_w)
+        command_vec = np.concatenate(
+            (
+                command_joint_pos.squeeze(0),
+                command_joint_vel.squeeze(0),
+                # ref_anchor_pos_w,
+                # command_root[:, :2].reshape(-1),
+            ),
+            dtype=np.float32,
+        )
+
+        mimic_obs_buf = np.concatenate(
+            (
+                command_vec,
+                motion_anchor_ori_b[:, :2].reshape(-1),
+                ang_vel,
+                qj,
+                dqj[self.mj2lab],
+                np.asarray(action_for_history, dtype=np.float32).reshape(-1),
+            ),
+            axis=-1,
+            dtype=np.float32,
+        )
+        if mimic_obs_buf.shape[0] != self.num_obs:
+            raise RuntimeError(f"Observation length mismatch. Expected {self.num_obs}, got {mimic_obs_buf.shape[0]}.")
+        
+        if torch is not None:
+            mimic_obs_tensor = torch.from_numpy(mimic_obs_buf).unsqueeze(0).cpu().numpy()
+        else:
+            mimic_obs_tensor = np.expand_dims(mimic_obs_buf, axis=0)
+        observation = {}
+
+        # obs0 是网络观测，obs1 是当前时间步，用于输出参考动作信息
+        observation[self.input_name[0]] = mimic_obs_tensor
+        time_index = max(self.counter_step - self.warm_start_steps, 0)
+        
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if self.motion_length is not None and self.motion_length > 0:
+                time_index = min(time_index, self.motion_length - 1)
+
+        observation[self.input_name[1]] = np.array([[time_index]], dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if time_index == self.motion_length - 1 and not self._final_ref_cached:
+                self._cache_final_ref()
+            elif self.counter_step >= self.motion_length and self._final_ref_cached:
+                self._apply_final_ref()
+        target_dof_pos_mj = np.zeros(29)
+        target_dof_pos_mj_19dof = np.zeros(19)
+        if use_warmstart and blended_target is not None:
+            target_dof_pos_lab = blended_target
+            # Keep action history aligned with the inserted warm trajectory.
+            self.action = np.asarray(action_for_history, dtype=np.float32).reshape(1, -1)
+        else:
+            target_dof_pos_lab = self.action * self.action_scale_lab + self.default_angles_lab
+            if target_dof_pos_lab.ndim > 1:
+                target_dof_pos_lab = np.squeeze(target_dof_pos_lab, axis=0)
+
+        if self.warm_start_steps > 0:
+            self._warmup_inference_counter += 1
+            if self._warmup_inference_counter <= self.warm_start_steps:
+                blend = self._warmup_inference_counter / self.warm_start_steps
+                if not use_warmstart:
+                    target_dof_pos_lab = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+
+        target_dof_pos_mj_19dof[self.mj2lab] = target_dof_pos_lab
+        target_dof_pos_mj[self.locked_joint_map] = target_dof_pos_mj_19dof
+
+        # Set joint commands exactly like C++
+        for i in range(self.motor_nums):
+            # C++: robot_data_->q_d_(35 - motor_num_ + i)
+            joint_idx = 35 - self.motor_nums + i
+            self.robot_data_.q_d_[joint_idx] = target_dof_pos_mj[i]
+            self.robot_data_.q_dot_d_[joint_idx] = 0.0
+            self.robot_data_.tau_d_[joint_idx] = 0.0
+
+        # update motion phase
+        self.counter_step += 1
+
+    def _cache_final_ref(self):
+        if not self.hold_final_reference:
+            return
+        self._final_ref_cached = True
+        self._final_ref_joint_pos = np.array(self.ref_joint_pos, copy=True)
+        self._final_ref_joint_vel = np.array(self.ref_joint_vel, copy=True)
+        self._final_ref_body_pos_w = np.array(self.ref_body_pos_w, copy=True)
+        self._final_ref_body_quat_w = np.array(self.ref_body_quat_w, copy=True)
+        self._final_ref_body_lin_vel_w = np.array(self.ref_body_lin_vel_w, copy=True)
+        self._final_ref_body_ang_vel_w = np.array(self.ref_body_ang_vel_w, copy=True)
+
+    def _apply_final_ref(self):
+        if not self.hold_final_reference or not self._final_ref_cached:
+            return
+        self.ref_joint_pos = np.array(self._final_ref_joint_pos, copy=True)
+        self.ref_joint_vel = np.array(self._final_ref_joint_vel, copy=True)
+        self.ref_body_pos_w = np.array(self._final_ref_body_pos_w, copy=True)
+        self.ref_body_quat_w = np.array(self._final_ref_body_quat_w, copy=True)
+        self.ref_body_lin_vel_w = np.array(self._final_ref_body_lin_vel_w, copy=True)
+        self.ref_body_ang_vel_w = np.array(self._final_ref_body_ang_vel_w, copy=True)
+
+    def run(self, flag: ControlFlag):
+        if int(self.robot_data_.time_now_ / self.physical_dt) % self.decimation_ == 0:
+            current_time = time.perf_counter()
+            print(f"Inference hz: {1/(current_time - self.last_run_time)}")
+            self.last_run_time = current_time
+            self.inner_run()
+        self.set_kp_kd()
+    def set_kp_kd(self):
+        # Set kp/kd gains
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+    def on_exit(self):
+        self.action = np.zeros(self.num_actions, dtype=np.float32)
+        # self.action_buf = np.zeros(23 * self.history_length, dtype=np.float32)
+        self.ref_motion_phase = 0.
+        # self.ref_motion_phase_buf = np.zeros(1 * self.history_length, dtype=np.float32)
+        self.motion_time = 0
+        self.counter_step = 0
+        self._final_ref_cached = False
+        
+        print("exited")
+
+    
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        else:
+            return None  # 无状态转换
+
+    def _get_ref_anchor_pose(self):
+        ref_pos = self.ref_body_pos_w[:, self.anchor_body_index].squeeze(0)
+        ref_quat = self.ref_body_quat_w[:, self.anchor_body_index].squeeze(0)
+        return ref_pos.astype(np.float32), ref_quat.astype(np.float32)
+
+    def _get_current_joint_pos_lab(self) -> np.ndarray:
+        try:
+            current_q = self.robot_data_.get_joint_pos()
+            current_q = current_q[self.locked_joint_map]
+            current_q_lab = current_q[self.mj2lab]
+            return current_q_lab.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read current joint pose: {exc}")
+            return np.array(self.default_angles_lab, copy=True)
+
+    def _get_onnx_first_frame_lab(self) -> np.ndarray:
+        try:
+            action = self.action
+            if action is None:
+                raise RuntimeError("ONNX action output is None")
+            if action.ndim > 1:
+                action = np.squeeze(action, axis=0)
+            first_frame = action * self.action_scale_lab + self.default_angles_lab
+            return first_frame.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read ONNX first frame: {exc}")
+            return np.array(self.default_angles_lab, copy=True)

Deploy_Tienkung/policy/beyondzero/__init__.py

@@ -0,0 +1,2 @@
+"""BeyondZero policy package."""
+

Deploy_Tienkung/policy/beyondzero/config/beyondzero.yaml

@@ -0,0 +1,30 @@
+onnx_path: "policy_gui6_1.onnx"
+
+motor_nums: 29
+interp_step: 0.01
+interp_max: 1.0
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 16, 17, 18,
+ 22, 23, 24, 25
+]
+
+kps: [
+        400, 400, 250, 350, 30, 16.8,
+        400, 400, 250, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,150,200,200,
+        150, 50, 50, 150,150,200,200,
+]
+
+kds: [
+        10, 10, 5, 10, 2.5, 1.4,
+        10, 10, 5, 10, 2.5, 1.4,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+

Deploy_Tienkung/policy/beyondzero/fsm_beyondzero.py

@@ -0,0 +1,121 @@
+"""
+BeyondZero FSM state
+Moves the robot to the BeyondMimic default pose using smooth interpolation.
+"""
+import os
+from typing import List
+
+import numpy as np
+import yaml
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateBeyondZero(FSMState):
+    """Zero pose specifically aligned with the BeyondMimic policy, with explicitly specified 29-joint target positions."""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.BEYONDZERO
+        self.q_factor = 0.0
+        self.motor_nums = 29
+        self.start_pose = np.zeros(self.motor_nums, dtype=np.float32)
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "beyondzero.yaml")
+        with open(config_path, "r") as f:
+            config = yaml.safe_load(f)
+
+        self.motor_nums: int = int(config["motor_nums"])
+        self.locked_joint_map: List[int] = config["locked_joint_map"]
+        self.kps = np.array(config["kps"], dtype=np.float32)
+        self.kds = np.array(config["kds"], dtype=np.float32)
+        self.interp_step = float(config.get("interp_step", 0.001))
+        self.interp_max = float(config.get("interp_max", 1.0))
+
+        # Explicitly define the 29-joint zero target positions
+        # These values are based on the default joint positions expanded to 29 elements.
+        # Adjust the array below as needed to specify your desired initial target positions for all 29 joints.
+        # For example, positions for controllable joints are set from defaults, others to 0.0.
+        self.zero_target = np.array([
+            -0.94777486,  # hip_pitch_l_joint
+            0.15228635,   # hip_roll_l_joint
+            -0.11064088,   # hip_yaw_l_joint
+            0.70493567,   # knee_pitch_l_joint
+            -0.07427792,  # ankle_pitch_l_joint
+            -0.08085743,   # ankle_roll_l_joint
+            -0.94510548,  # hip_pitch_r_joint
+            -0.19115149,   # hip_roll_r_joint
+            0.01340432,   # hip_yaw_r_joint
+            0.79190012,   # knee_pitch_r_joint
+            -0.19874191,  # ankle_pitch_r_joint
+            0.02820061,   # ankle_roll_r_joint
+            -0.0,   # waist_yaw_joint
+            -0.0,   # waist_roll_joint
+            0.17578462,   # waist_pitch_joint
+            -1.01426013,   # shoulder_pitch_l_joint
+            0.000,  # shoulder_roll_l_joint (adjusted example)
+            0.000,   # shoulder_yaw_l_joint
+            -1.78047789,   # elbow_pitch_l_joint
+            0.0,
+            0.0,
+            0.0,
+            -1.01426013,   # shoulder_pitch_r_joint
+            0.000,   # shoulder_roll_r_joint
+            0.000,   # shoulder_yaw_r_joint
+            -1.78047789,   # elbow_pitch_r_joint
+            0.000,   # Additional joints (e.g., wrists or others, set to 0.0)
+            0.000,
+            0.000    # Last joint
+        ], dtype=np.float32)
+
+        # If preferred, load from config instead of hardcoding:
+        # self.zero_target = np.array(config.get("zero_target", self.zero_target.tolist()), dtype=np.float32)
+
+        if len(self.zero_target) != self.motor_nums:
+            raise ValueError(f"Specified zero_target length ({len(self.zero_target)}) does not match motor_nums ({self.motor_nums}).")
+
+    def on_enter(self):
+        print("[FSMStateBeyondZero] Enter zero pose with specified 29-joint targets")
+        self.q_factor = 0.0
+        if self.robot_data_ is not None:
+            self.start_pose = self.robot_data_.get_joint_pos().copy()
+        else:
+            self.start_pose = np.zeros(self.motor_nums, dtype=np.float32)
+
+    def run(self, flag: ControlFlag):
+        if self.robot_data_ is None:
+            return
+
+        target = self.zero_target
+        if self.q_factor < self.interp_max:
+            pos_cmd = (1.0 - self.q_factor) * self.start_pose + self.q_factor * target
+            self.q_factor = min(self.q_factor + self.interp_step, self.interp_max)
+        else:
+            pos_cmd = target
+
+        joint_start_idx = 35 - self.motor_nums
+        self.robot_data_.q_d_[joint_start_idx:] = pos_cmd
+        self.robot_data_.q_dot_d_[joint_start_idx:] = 0.0
+        self.robot_data_.tau_d_[joint_start_idx:] = 0.0
+
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+
+    def on_exit(self):
+        print("[FSMStateBeyondZero] Exit BeyondZero state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """Allow transitions to other FSM states."""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None

Deploy_Tienkung/policy/mypolicy/config/mypolicy.yaml

@@ -0,0 +1,96 @@
+model_path: "policy.onnx"
+motor_num: 29
+actions_size: 23
+dt: 0.01
+warm_start_time: 0.0
+xsense_data_roll_offset: 0.0
+joint_names: [
+  hip_pitch_l_joint, hip_pitch_r_joint, waist_yaw_joint,
+  hip_roll_l_joint, hip_roll_r_joint, waist_roll_joint,
+  hip_yaw_l_joint, hip_yaw_r_joint, waist_pitch_joint,
+  knee_pitch_l_joint, knee_pitch_r_joint,
+  shoulder_pitch_l_joint, shoulder_pitch_r_joint,
+  ankle_pitch_l_joint, ankle_pitch_r_joint,
+  shoulder_roll_l_joint, shoulder_roll_r_joint,
+  ankle_roll_l_joint, ankle_roll_r_joint,
+  shoulder_yaw_l_joint, shoulder_yaw_r_joint,
+  elbow_pitch_l_joint, elbow_pitch_r_joint
+]
+zero_pos_offset: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6,
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6,
+          3.207, 3.207, 3.207,
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3,
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+control:
+  action_scale: 0.25
+  decimation: 2
+
+gait:
+  gait_air_ratio_l: 0.6
+  gait_air_ratio_r: 0.6
+  gait_phase_offset_l: 0.6
+  gait_phase_offset_r: 0.1
+  gait_cycle: 0.64
+
+normalization:
+  clip_scales:
+    clip_observations: 100.0
+    clip_actions: 100.0
+  obs_scales:
+    lin_vel: 1.0
+    ang_vel: 1.0
+    dof_pos: 1.0
+    dof_vel: 1.0
+
+size:
+  num_hist: 10
+  observations_size: 84
+
+gains:
+  kp: [
+    300.0, 300.0, 400.0,
+    300.0, 300.0, 400.0,
+    150.0, 150.0, 400.0,
+    350.0, 350.0,
+    150.0, 150.0,
+    30.0, 30.0,
+    50.0, 50.0,
+    16.8, 16.8,
+    50.0, 50.0,
+    150.0, 150.0
+  ]
+  kd: [
+    10.0, 10.0, 5.0,
+    10.0, 10.0, 10.0,
+    5.0, 5.0, 10.0,
+    10.0, 10.0,
+    7.5, 7.5,
+    2.5, 2.5,
+    2.5, 2.5,
+    1.4, 1.4,
+    2.5, 2.5,
+    5.0, 5.0
+  ]
+
+init_state:
+  default_joint_angles: [
+    0.0, 0.0, 0.0,
+    -0.5, -0.5, 0.0,
+    0.0, 0.0, 0.0,
+    1.0, 1.0,
+    0.0, 0.0,
+    -0.5, -0.5,
+    0.2, -0.2,
+    0.0, 0.0,
+    0.0, 0.0,
+    -0.3, -0.3
+  ]

Deploy_Tienkung/policy/mypolicy/fsm_mypolicy.py

@@ -0,0 +1,373 @@
+"""
+FSM state implementation for the standalone MYPOLICY controller.
+"""
+
+import os
+
+import numpy as np
+import onnxruntime as ort
+import yaml
+from scipy.spatial.transform import Rotation
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.BasicFunction import gait_phase
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+
+
+class FSMStateMYPOLICY(FSMState):
+    """Standalone FSM implementation for the custom ONNX policy."""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.MYPOLICY
+        self.log_prefix = "FSMStateMYPOLICY"
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "mypolicy.yaml")
+        with open(config_path, "r") as f:
+            policy_config = yaml.safe_load(f)
+
+        self.action_num_ = policy_config.get("actions_size")
+        self.motor_num_ = policy_config.get("motor_num")
+        self.dt_ = policy_config.get("dt")
+
+        size_config = policy_config.get("size", {})
+        self.num_hist_ = size_config.get("num_hist")
+        self.obs_size_ = size_config.get("observations_size")
+
+        control_config = policy_config.get("control", {})
+        self.action_scale_ = control_config.get("action_scale")
+        self.decimation_ = control_config.get("decimation")
+        self.warm_start_time_ = control_config.get(
+            "warm_start_time",
+            policy_config.get("warm_start_time", 0.3),
+        )
+
+        norm_config = policy_config.get("normalization", {})
+        clip_config = norm_config.get("clip_scales", {})
+        obs_config = norm_config.get("obs_scales", {})
+        self.clip_obs_ = clip_config.get("clip_observations", 100.0)
+        self.clip_act_ = clip_config.get("clip_actions", 100.0)
+        self.lin_vel_scale_ = obs_config.get("lin_vel")
+        self.ang_vel_scale_ = obs_config.get("ang_vel")
+        self.dof_pos_scale_ = obs_config.get("dof_pos")
+        self.dof_vel_scale_ = obs_config.get("dof_vel")
+
+        self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.proprio_hist_buf_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self._warm_start_pose = np.zeros(self.motor_num_, dtype=np.float32)
+
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+        self.timer_gait_ = 0.0
+
+        gait_config = policy_config.get("gait", {})
+        self.gait_cycle = gait_config.get("gait_cycle", 0.64)
+        self.left_phase_ratio = gait_config.get("gait_air_ratio_l", 0.6)
+        self.right_phase_ratio = gait_config.get("gait_air_ratio_r", 0.6)
+        self.left_theta_offset = gait_config.get("gait_phase_offset_l", 0.6)
+        self.right_theta_offset = gait_config.get("gait_phase_offset_r", 0.1)
+
+        step = (self.decimation_ if self.decimation_ else 1) * self.dt_
+        if self.warm_start_time_ > 0 and step > 0:
+            self._warm_start_steps = max(1, int(self.warm_start_time_ / step))
+        else:
+            self._warm_start_steps = 0
+        self._warmup_inference_counter = 0
+
+        self.waiting_for_motion = True
+        self.motion_threshold = 1e-3
+        self.hold_pose = np.zeros(self.motor_num_, dtype=np.float32)
+        self.filtered_x_speed = 0.0
+
+        self.model_path = os.path.join(current_dir, "model", policy_config["model_path"])
+        self._init_onnx_session()
+
+        joint_names = policy_config.get("joint_names")
+        if joint_names is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'joint_names' in mypolicy.yaml")
+        self.joint_seq = list(joint_names)
+
+        if self.action_scale_ is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'control.action_scale' in mypolicy.yaml")
+        if np.isscalar(self.action_scale_):
+            self.action_scale = np.full(len(self.joint_seq), float(self.action_scale_), dtype=np.float32)
+        else:
+            self.action_scale = np.array(self.action_scale_, dtype=np.float32)
+        if len(self.action_scale) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] control.action_scale length {len(self.action_scale)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        init_state_config = policy_config.get("init_state", {})
+        default_joint_angles = init_state_config.get("default_joint_angles")
+        if default_joint_angles is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'init_state.default_joint_angles' in mypolicy.yaml")
+        self.joint_pos_array_seq = np.array(default_joint_angles, dtype=np.float32)
+        if len(self.joint_pos_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] init_state.default_joint_angles length {len(self.joint_pos_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        gains_config = policy_config.get("gains", {})
+        kp_values = gains_config.get("kp")
+        kd_values = gains_config.get("kd")
+        if kp_values is None or kd_values is None:
+            raise ValueError("[FSMStateMYPOLICY] Missing 'gains.kp' or 'gains.kd' in mypolicy.yaml")
+        self.stiffness_array_seq = np.array(kp_values, dtype=np.float32)
+        self.damping_array_seq = np.array(kd_values, dtype=np.float32)
+        if len(self.stiffness_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] gains.kp length {len(self.stiffness_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        if len(self.damping_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateMYPOLICY] gains.kd length {len(self.damping_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        self.joint_xml = [
+            "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+            "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+            "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+            "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+            "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+            "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+            "elbow_pitch_l_joint", "elbow_yaw_l_joint", "wrist_pitch_l_joint", "wrist_roll_l_joint",
+            "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+            "elbow_pitch_r_joint", "elbow_yaw_r_joint", "wrist_pitch_r_joint", "wrist_roll_r_joint",
+        ]
+
+        self.lab2mj = []
+        for name in self.joint_seq:
+            if name not in self.joint_xml:
+                raise ValueError(f"[FSMStateMYPOLICY] joint '{name}' from mypolicy.yaml not found in joint_xml")
+            self.lab2mj.append(self.joint_xml.index(name))
+        self.lab2mj = np.array(self.lab2mj, dtype=int)
+
+        n_mj = len(self.joint_xml)
+        self.joint_pos_array = np.zeros(n_mj, dtype=np.float32)
+        self.stiffness_array = np.zeros(n_mj, dtype=np.float32)
+        self.damping_array = np.zeros(n_mj, dtype=np.float32)
+        for lab_idx, mj_idx in enumerate(self.lab2mj):
+            self.joint_pos_array[mj_idx] = self.joint_pos_array_seq[lab_idx]
+            self.stiffness_array[mj_idx] = self.stiffness_array_seq[lab_idx]
+            self.damping_array[mj_idx] = self.damping_array_seq[lab_idx]
+
+        self.kps_lab = self.stiffness_array_seq
+        self.kds_lab = self.damping_array_seq
+        self.default_angles_lab = self.joint_pos_array_seq
+        self.action_scale_lab = self.action_scale
+
+    def _init_onnx_session(self):
+        try:
+            options = ort.SessionOptions()
+            options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+            options.intra_op_num_threads = 1
+            options.inter_op_num_threads = 1
+            options.enable_mem_pattern = False
+            options.enable_mem_reuse = True
+            self.ort_session_ = ort.InferenceSession(
+                self.model_path,
+                options,
+                providers=["CPUExecutionProvider"],
+            )
+            print(f"[{self.log_prefix}-ONNX] ONNX model loaded successfully: {self.model_path}")
+        except Exception as e:
+            print(f"[{self.log_prefix}] Failed to load ONNX model: {e}")
+            self.ort_session_ = None
+
+    def _reset_internal_state(self):
+        self.observations_.fill(0.0)
+        self.proprio_hist_buf_.fill(0.0)
+        self.last_actions_.fill(0.0)
+        self.actions_.fill(0.0)
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+        self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.joint_pos_array
+        self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+        self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+
+    def on_enter(self):
+        self._reset_internal_state()
+        print(f"[{self.log_prefix}] enter")
+        self.timer_gait_ = 0.0
+        self.waiting_for_motion = True
+        self._warmup_inference_counter = 0
+        if self.robot_data_ is not None:
+            try:
+                current_pose = self.robot_data_.get_joint_pos().copy()
+                self._warm_start_pose = current_pose
+                self.hold_pose = current_pose
+            except Exception:
+                self._warm_start_pose.fill(0.0)
+                self.hold_pose.fill(0.0)
+        else:
+            self._warm_start_pose.fill(0.0)
+            self.hold_pose.fill(0.0)
+        print(f"[{self.log_prefix}] waiting for motion command before starting policy")
+
+    def run(self, flag: ControlFlag):
+        walk_cmd = np.array(self.robot_data_.get_walk_cmd(), dtype=np.float32)
+        if self.waiting_for_motion:
+            if np.max(np.abs(walk_cmd)) <= self.motion_threshold:
+                base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+                self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.hold_pose
+                self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+                self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+                self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+                self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+                return
+            self.waiting_for_motion = False
+            self._warm_start_pose = self.robot_data_.get_joint_pos().copy()
+            self._warmup_inference_counter = 0
+            print(f"[{self.log_prefix}] motion command detected: {walk_cmd}, policy activated")
+
+        print(f"[{self.log_prefix}] run")
+        gait = gait_phase(
+            self.timer_gait_,
+            self.gait_cycle,
+            self.left_theta_offset,
+            self.right_theta_offset,
+            self.left_phase_ratio,
+            self.right_phase_ratio,
+        ).astype(np.float32)
+
+        if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
+            self.compute_observation(flag, gait)
+            self.compute_actions()
+
+            target_dof_pos_lab = self.actions_ * self.action_scale_lab + self.default_angles_lab
+            target_dof_pos_mj = self.robot_data_.get_joint_pos().copy()
+            target_dof_pos_mj[self.lab2mj] = target_dof_pos_lab
+            commanded_pos = target_dof_pos_mj
+            if self._warm_start_steps > 0 and self._warmup_inference_counter < self._warm_start_steps:
+                self._warmup_inference_counter += 1
+                blend = self._warmup_inference_counter / float(self._warm_start_steps)
+                commanded_pos = (1.0 - blend) * self._warm_start_pose + blend * target_dof_pos_mj
+
+            base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+            self.robot_data_.q_d_[base:base + len(self.joint_xml)] = commanded_pos
+            self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+            self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+            self.last_actions_[:] = self.actions_
+
+        self.timer_gait_ += self.dt_
+        self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+        self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+
+    def compute_observation(self, flag: ControlFlag, gait):
+        roll, pitch, yaw = (
+            float(self.robot_data_.imu_data_[2]),
+            float(self.robot_data_.imu_data_[1]),
+            float(self.robot_data_.imu_data_[0]),
+        )
+        quat_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
+        q_xyzw = np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]], dtype=np.float32)
+        gravity_init = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.0, 0.0, -1.0], dtype=np.float32))
+
+        x_speed_command, y_speed_command, yaw_speed_command = self.robot_data_.get_walk_cmd()
+        new_filtered_x_speed = x_speed_command
+        change = new_filtered_x_speed - self.filtered_x_speed
+        change = np.clip(change, -0.005, 0.005)
+        self.filtered_x_speed = self.filtered_x_speed + change
+        command = np.array(
+            [x_speed_command, y_speed_command, yaw_speed_command],
+            dtype=np.float32,
+        )
+        print(f"Input command: {command}")
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+        q_mj = self.robot_data_.get_joint_pos()
+        dq_mj = self.robot_data_.get_joint_vel()
+        qj = q_mj[self.lab2mj] - self.default_angles_lab
+        dqj = dq_mj[self.lab2mj]
+
+        proprio = np.concatenate([
+            ang_vel,
+            gravity_init,
+            command,
+            qj,
+            dqj,
+            self.last_actions_,
+            gait,
+        ])
+
+        if self.is_first_obs_:
+            for i in range(self.num_hist_):
+                start_idx = i * self.obs_size_
+                end_idx = start_idx + self.obs_size_
+                self.proprio_hist_buf_[start_idx:end_idx] = proprio
+            self.is_first_obs_ = False
+        else:
+            shift_size = (self.num_hist_ - 1) * self.obs_size_
+            self.proprio_hist_buf_[:shift_size] = self.proprio_hist_buf_[self.obs_size_:]
+            self.proprio_hist_buf_[shift_size:] = proprio
+
+        self.observations_ = np.clip(self.proprio_hist_buf_, -self.clip_obs_, self.clip_obs_)
+
+    def compute_actions(self):
+        if self.ort_session_ is None:
+            return
+
+        try:
+            input_data = self.observations_.reshape(1, -1).astype(np.float32)
+            input_name = self.ort_session_.get_inputs()[0].name
+            outputs = self.ort_session_.run(None, {input_name: input_data})
+            output_data = outputs[0][0]
+            for i in range(self.action_num_):
+                self.actions_[i] = np.clip(output_data[i], -self.clip_act_, self.clip_act_)
+
+            if self.is_first_action_:
+                print(f"[{self.log_prefix}-ONNX] First Observation:")
+                for i in range(self.obs_size_):
+                    print(f"{self.observations_[i]:.6f} ", end="")
+                print()
+                self.is_first_action_ = False
+        except Exception as e:
+            print(f"[{self.log_prefix}] ONNX Runtime inference error: {e}")
+
+    def on_exit(self):
+        print(f"[{self.log_prefix}] exit")
+        if getattr(self, "obs_log_file", None) is not None:
+            try:
+                self.obs_log_file.flush()
+                self.obs_log_file.close()
+                print(f"[{self.log_prefix}] obs log saved to {self.obs_log_path}")
+            except Exception as e:
+                print(f"[{self.log_prefix}] failed to close obs log: {e}")
+            self.obs_log_file = None
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        if flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        if flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        if flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        if flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        return None
+
+    @staticmethod
+    def euler_to_quaternion_scipy(roll, pitch, yaw, degrees=False):
+        r = Rotation.from_euler("xyz", [roll, pitch, yaw], degrees=degrees)
+        q_xyzw = r.as_quat()
+        return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]], dtype=np.float32)
+
+    @staticmethod
+    def quat_rotate_inverse_numpy(q_xyzw, v):
+        q_w = q_xyzw[3]
+        q_v = q_xyzw[:3]
+        a = v * (2.0 * q_w * q_w - 1.0)
+        b = np.cross(q_v, v) * (2.0 * q_w)
+        c = q_v * (2.0 * np.dot(q_v, v))
+        return a - b + c

Deploy_Tienkung/policy/niukua/config/niukua.yaml

@@ -0,0 +1,117 @@
+# onnx_path: "policy_fightAndSports1_s1.onnx"
+# onnx_path: "policy-dance.onnx"
+onnx_path: "niukua_evt1216.onnx"
+warm_start_time: 0.0
+
+body_names : [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "elbow_pitch_r_link",
+]
+anchor_body : "pelvis"
+
+#without motion anchor pos && anchor base lin vel
+# command :23+23+3+6  55
+# motion_anchor_ori_b: 6
+# base_ang_vel: 3
+# joint_pos（相对轴角）: 23
+# joint_vel: 23
+# actions（上一时刻动作）: 23
+# total : 133
+num_obs : 133 
+
+locked_joint_map : [
+ 0, 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10, 11,
+ 12, 13, 14,
+ 15, 16, 17, 18, 
+ 22, 23, 24, 25 
+]
+
+kps: [
+        300, 300, 150, 350, 30, 16.8,
+        300, 300, 150, 350, 30, 16.8,
+        400, 400, 400,
+        150, 50, 50, 150,500,200,200,
+        150, 50, 50, 150,500,200,200,
+]
+kds: [
+        10, 10, 5, 10, 5.0, 2.0,
+        10, 10, 5, 10, 5.0, 2.0,
+        5, 10, 10,
+        5, 2.5, 2.5, 5,5,2,2,
+        5, 2.5, 2.5, 5,5,2,2,
+]
+# motion_length: 140 
+
+# tau_limit: [88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                    88.0,  88.0, 88.0,
+#                   139.0, 139.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                    50.0,  50.0      , 25.0, 25.0,
+#                                       25.0, 25.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0,
+#                                        5.0,  5.0
+#            ]
+
+# kp_lab: [40.179, 40.179, 40.179, 99.098, 99.098, 28.501, 
+#           40.179, 40.179, 28.501, 99.098, 99.098, 14.251, 
+#           14.251, 28.501, 28.501, 14.251, 14.251, 28.501, 
+#           28.501, 14.251, 14.251, 14.251, 14.251, 14.251, 
+#           14.251, 16.778, 16.778, 16.778, 16.778]
+
+# kd_lab: [2.558, 2.558, 2.558, 6.309, 6.309, 1.814, 
+#           2.558, 2.558, 1.814, 6.309, 6.309, 0.907, 
+#           0.907, 1.814, 1.814, 0.907, 0.907, 1.814, 
+#           1.814, 0.907, 0.907, 0.907, 0.907, 0.907, 
+#           0.907, 1.068, 1.068, 1.068, 1.068]
+
+# default_angles_lab: [-0.312, -0.312, 0.0, 0.0, 0.0, 0.0, 
+#                     0.0, 0.0, 0.0, 0.669, 0.669, 0.2, 
+#                     0.2, -0.363, -0.363, 0.2, -0.2, 0.0, 
+#                     0.0, 0.0, 0.0, 0.6, 0.6, 0.0, 
+#                     0.0, 0.0, 0.0, 0.0, 0.0]
+
+# action_scale_lab: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 
+#                   0.548, 0.548, 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, 0.439, 0.439, 0.439, 
+#                   0.439, 0.075, 0.075, 0.075, 0.075]
+
+num_actions: 23
+motor_nums: 29
+# num_obs: 154
+hold_final_reference: false
+
+# mj2lab: [0, 6, 12,
+#                   1, 7, 13,
+#                   2, 8, 14,
+#                   3, 9    , 15, 22,
+#                   4, 10   , 16, 23,
+#                   5, 11   , 17, 24,
+#                             18, 25,
+#                             19, 26,
+#                             20, 27,
+#                             21, 28]
+# history_length: 4
+physical_dt: 0.01
+decimation: 1

Deploy_Tienkung/policy/niukua/fsm_beyond_mimic.py

@@ -0,0 +1,548 @@
+from FSM.fsm_base import FSMState, FSMStateName
+import numpy as np
+import yaml
+import os
+from types import SimpleNamespace
+from typing import Optional
+try:
+    import onnx
+except ImportError:  # pragma: no cover - runtime fallback when onnx isn't installed
+    onnx = None
+import onnxruntime
+try:
+    import torch
+except ImportError:  # pragma: no cover - torch is optional for warm start prep
+    torch = None
+from common.robot_data import RobotData
+from common.joystick import ControlFlag
+import time
+
+
+DEFAULT_BODY_NAMES = [
+    "pelvis",
+    "hip_pitch_l_link",
+    "hip_roll_l_link",
+    "hip_yaw_l_link",
+    "knee_pitch_l_link",
+    "ankle_pitch_l_link",
+    "ankle_roll_l_link",
+    "hip_pitch_r_link",
+    "hip_roll_r_link",
+    "hip_yaw_r_link",
+    "knee_pitch_r_link",
+    "ankle_pitch_r_link",
+    "ankle_roll_r_link",
+    "waist_yaw_link",
+    "waist_roll_link",
+    "waist_pitch_link",
+    "shoulder_pitch_l_link",
+    "shoulder_roll_l_link",
+    "shoulder_yaw_l_link",
+    "elbow_pitch_l_link",
+    "shoulder_pitch_r_link",
+    "shoulder_roll_r_link",
+    "shoulder_yaw_r_link",
+    "elbow_pitch_r_link",
+]
+
+class FSMStateBeyondMimic(FSMState):
+    def __init__(self, robot_data: RobotData, config_path: Optional[str] = None, variant_name: str = "default"):
+        super().__init__(robot_data)
+        self.motion_phase = 0
+        self.counter_step = 0
+        self.ref_motion_phase = 0
+        self.variant_name = variant_name
+        
+        
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "BeyondMimic.yaml")
+        self.config_path = os.path.abspath(config_path)
+        with open(self.config_path, "r") as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+            # 兼容多策略：模型路径仍然默认指向 policy/beyond_mimic/model 下
+            self.onnx_path = config["onnx_path"]
+            if not os.path.isabs(self.onnx_path):
+                self.onnx_path = os.path.join(current_dir, "model", self.onnx_path)
+            # self.motion_length = config["motion_length"]
+            # self.history_length = config["history_length"]
+            self.physical_dt = config["physical_dt"]
+            self.decimation_ = config["decimation"]
+            self.num_actions = config["num_actions"]
+            self.motor_nums = config["motor_nums"]
+            self.warm_start_time = config["warm_start_time"]
+            self.kps = config["kps"]
+            self.kds = config["kds"]
+            self.hold_final_reference = config.get("hold_final_reference", False)
+            self.motion_length: Optional[int] = config.get("motion_length")
+            self.body_names = config.get("body_names", DEFAULT_BODY_NAMES)
+            self.locked_joint_map = config["locked_joint_map"]
+            self.anchor_body_name = config.get("anchor_body", "pelvis")
+            if self.anchor_body_name not in self.body_names:
+                raise ValueError(f"Anchor body {self.anchor_body_name} missing from body list.")
+            self.anchor_body_index = self.body_names.index(self.anchor_body_name)
+            self.num_bodies = len(self.body_names)
+            self._warm_start_from_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_to_lab = np.zeros(self.num_actions, dtype=np.float32)
+            self._warm_start_prev_target = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.last_run_time = time.perf_counter()
+            
+            self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)  # 初始化为最大可能大小
+            self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+            self.num_obs = None  # set after loading onnx
+            self.obs = None
+            self.action = np.zeros(self.num_actions, dtype=np.float32)
+
+            self.ref_joint_pos = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_joint_vel = np.zeros(self.num_actions, dtype=np.float32)
+            self.ref_body_pos_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_quat_w = np.zeros((1, self.num_bodies, 4), dtype=np.float32)
+            self.ref_body_lin_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            self.ref_body_ang_vel_w = np.zeros((1, self.num_bodies, 3), dtype=np.float32)
+            # load policy
+            self.onnx_model = None
+            metadata_props = []
+            if onnx is not None and hasattr(onnx, "load"):
+                try:
+                    self.onnx_model = onnx.load(self.onnx_path)
+                    metadata_props = getattr(self.onnx_model, "metadata_props", [])
+                except Exception as exc:
+                    print(f"[FSMStateBeyondMimic] Failed to load ONNX model via onnx.load: {exc}")
+            else:
+                print("[FSMStateBeyondMimic] Python onnx package unavailable, falling back to onnxruntime metadata.")
+
+            self.ort_session = onnxruntime.InferenceSession(self.onnx_path)
+            if not metadata_props:
+                model_meta = self.ort_session.get_modelmeta()
+                custom_map = getattr(model_meta, "custom_metadata_map", {})
+                metadata_props = [SimpleNamespace(key=k, value=v) for k, v in custom_map.items()]
+
+            input = self.ort_session.get_inputs()
+            self.input_name = []
+            for i, inpt in enumerate(input):
+                self.input_name.append(inpt.name)
+            obs_input = self.ort_session.get_inputs()[0]
+            last_dim = obs_input.shape[-1]
+            if isinstance(last_dim, int):
+                self.num_obs = last_dim
+            else:
+                self.num_obs = config.get("num_obs", 154)
+            self.obs = np.zeros(self.num_obs, dtype=np.float32)
+
+            # 从ONNX模型中读取参数
+            self.joint_seq = None
+            self.joint_pos_array_seq = None
+            self.action_scale = None
+            # self.stiffness_array_seq = None
+            # self.damping_array_seq = None
+            
+            for prop in metadata_props:
+                if prop.key == "joint_names":
+                    self.joint_seq = prop.value.split(",")
+                if prop.key == "default_joint_pos":   
+                    self.joint_pos_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_stiffness":
+                #     self.stiffness_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                # if prop.key == "joint_damping":
+                #     self.damping_array_seq = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key == "action_scale":
+                    self.action_scale = np.array([float(x) for x in prop.value.split(",")])
+                if prop.key in ("motion_length", "time_step_total"):
+                    try:
+                        self.motion_length = int(float(prop.value))
+                    except (TypeError, ValueError):
+                        print(f"[FSMStateBeyondMimic] Invalid motion_length metadata value: {prop.value}")
+
+            if self.motion_length is not None:
+                try:
+                    self.motion_length = int(self.motion_length)
+                except (TypeError, ValueError):
+                    print(f"[FSMStateBeyondMimic] Invalid motion_length config value: {self.motion_length}")
+                    self.motion_length = None
+            
+            # # 根据YAML配置设置关节映射
+            # self.mj2lab = np.array(config["mj2lab"], dtype=np.int32)
+            
+            # 设置从序列到实验室顺序的映射
+            self.joint_xml = [
+                "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+                "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+                "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+                "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+                "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+                "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+                "elbow_pitch_l_joint",
+                "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+                "elbow_pitch_r_joint",
+            ]
+            # 从MjXUML顺序映射到实验室顺序
+            self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+
+            # 从实验室顺序映射到MjXUML顺序
+            self.joint_pos_array = np.array([self.joint_pos_array_seq[self.joint_seq.index(joint)] for joint in self.joint_xml])
+
+            self.default_angles_lab = self.joint_pos_array_seq
+            self.action_scale_lab = self.action_scale
+
+            print("BeyondMimic-like policy initializing ...")
+            self._warmup_inference_counter = 0
+            self.warm_start_steps = 0
+            # Cache for the last motion frame so we can keep sending it after motion ends.
+            self._final_ref_cached = False
+            self._final_ref_joint_pos = np.zeros_like(self.ref_joint_pos)
+            self._final_ref_joint_vel = np.zeros_like(self.ref_joint_vel)
+            self._final_ref_body_pos_w = np.zeros_like(self.ref_body_pos_w)
+            self._final_ref_body_quat_w = np.zeros_like(self.ref_body_quat_w)
+            self._final_ref_body_lin_vel_w = np.zeros_like(self.ref_body_lin_vel_w)
+            self._final_ref_body_ang_vel_w = np.zeros_like(self.ref_body_ang_vel_w)
+    
+    def on_enter(self):
+        self.ref_motion_phase = 0.
+        self.motion_time = 0
+        self.counter_step = 0
+        self._warmup_inference_counter = 0
+        print(f"[FSMStateBeyondMimic] enter variant={self.variant_name}, config={self.config_path}")
+        if self.warm_start_time > 0:
+            step = self.decimation_ * self.physical_dt
+            self.warm_start_steps = max(1, int(self.warm_start_time / step))
+        else:
+            self.warm_start_steps = 0
+
+        observation = {}
+        observation[self.input_name[0]] = np.zeros((1, self.num_obs), dtype=np.float32)
+        observation[self.input_name[1]] = np.zeros((1, 1), dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        self.qj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.dqj_obs = np.zeros(self.num_actions, dtype=np.float32)
+        self.obs = np.zeros(self.num_obs)
+        self._final_ref_cached = False
+        self._warm_start_from_lab = self._get_current_joint_pos_lab()
+        self._warm_start_to_lab = self._get_onnx_first_frame_lab()
+        self._warm_start_prev_target = np.array(self._warm_start_from_lab, copy=True)
+
+        # self.action = np.zeros(self.num_actions)
+
+        pass
+        
+    def quat_mul(self, q1, q2):
+        w1, x1, y1, z1 = q1[0], q1[1], q1[2], q1[3]
+        w2, x2, y2, z2 = q2[0], q2[1], q2[2], q2[3]
+        # perform multiplication
+        ww = (z1 + x1) * (x2 + y2)
+        yy = (w1 - y1) * (w2 + z2)
+        zz = (w1 + y1) * (w2 - z2)
+        xx = ww + yy + zz
+        qq = 0.5 * (xx + (z1 - x1) * (x2 - y2))
+        w = qq - ww + (z1 - y1) * (y2 - z2)
+        x = qq - xx + (x1 + w1) * (x2 + w2)
+        y = qq - yy + (w1 - x1) * (y2 + z2)
+        z = qq - zz + (z1 + y1) * (w2 - x2)
+        return np.array([w, x, y, z])
+        
+    def matrix_from_quat(self, q):
+        w, x, y, z = q
+        return np.array([
+            [1 - 2 * (y**2 + z**2), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x**2 + z**2), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x**2 + y**2)]
+        ])
+
+    def yaw_quat(self, q):
+        w, x, y, z = q
+        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
+        return np.array([np.cos(yaw / 2), 0, 0, np.sin(yaw / 2)])
+    
+    def euler_single_axis_to_quat(self, angle, axis, degrees=False):
+        """
+        将单个欧拉角转换为四元数
+        
+        参数:
+            angle: 旋转角度
+            axis: 旋转轴，可以是 'x', 'y', 'z' 或者单位向量 [x, y, z]
+            degrees: 如果为True，输入角度为度数；如果为False，输入角度为弧度
+        
+        返回:
+            四元数 (w, x, y, z)
+        """
+        # 转换角度为弧度
+        if degrees:
+            angle = np.radians(angle)
+        
+        # 计算半角
+        half_angle = angle * 0.5
+        cos_half = np.cos(half_angle)
+        sin_half = np.sin(half_angle)
+        
+        # 根据旋转轴确定四元数分量
+        if isinstance(axis, str):
+            if axis.lower() == 'x':
+                return np.array([cos_half, sin_half, 0.0, 0.0])
+            elif axis.lower() == 'y':
+                return np.array([cos_half, 0.0, sin_half, 0.0])
+            elif axis.lower() == 'z':
+                return np.array([cos_half, 0.0, 0.0, sin_half])
+            else:
+                raise ValueError("axis must be 'x', 'y', 'z' or a 3D unit vector")
+        else:
+            # 假设axis是一个3D向量 [x, y, z]
+            axis = np.array(axis, dtype=np.float32)
+            # 归一化轴向量
+            axis_norm = np.linalg.norm(axis)
+            if axis_norm == 0:
+                raise ValueError("axis vector cannot be zero")
+            axis = axis / axis_norm
+            
+            # 计算四元数分量
+            w = cos_half
+            x = sin_half * axis[0]
+            y = sin_half * axis[1]
+            z = sin_half * axis[2]
+            
+            return np.array([w, x, y, z])
+
+    def inner_run(self):
+        robot_quat = self.robot_data_.get_robot_quat()
+        qj = self.robot_data_.get_joint_pos()
+        # 将29dof自由度的数据映射回锁住腕部6关节，之后的逻辑和和之前没区别
+        qj = qj[self.locked_joint_map]
+
+        qj = qj[self.mj2lab]
+        qj = (qj - self.default_angles_lab)
+
+        # IMU mounted on pelvis, so directly use measured orientation.
+        ref_anchor_pos_w, ref_anchor_ori_w = self._get_ref_anchor_pose()
+
+        # 在第一帧提取当前机器人yaw方向，与参考动作yaw方向做差（与beyond mimic一致）
+        if(self.counter_step < 2):
+            init_to_anchor = self.matrix_from_quat(self.yaw_quat(ref_anchor_ori_w))
+            world_to_anchor = self.matrix_from_quat(self.yaw_quat(robot_quat))
+            self.init_to_world = world_to_anchor @ init_to_anchor.T
+            print("self.init_to_world: ", self.init_to_world)
+            self.counter_step += 1
+            return
+
+        robot_rot_mat = self.matrix_from_quat(robot_quat)
+        motion_anchor_ori_b = robot_rot_mat.T @ self.init_to_world @ self.matrix_from_quat(ref_anchor_ori_w)
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+
+        dqj = self.robot_data_.get_joint_vel()
+        #映射到23dof
+        dqj = dqj[self.locked_joint_map]
+
+        use_warmstart = (
+            self.warm_start_steps > 0
+            and self._warmup_inference_counter < self.warm_start_steps
+        )
+        blended_target = None
+        if use_warmstart:
+            blend = (self._warmup_inference_counter + 1) / self.warm_start_steps
+            blended_target = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+            blended_vel = (blended_target - self._warm_start_prev_target) / (
+                self.decimation_ * self.physical_dt
+            )
+            self._warm_start_prev_target = blended_target
+
+            command_joint_pos = blended_target.reshape(1, -1)
+            command_joint_vel = blended_vel.reshape(1, -1)
+            safe_scale = np.where(self.action_scale_lab == 0, 1.0, self.action_scale_lab)
+            action_for_history = (blended_target - self.default_angles_lab) / safe_scale
+        else:
+            command_joint_pos = self.ref_joint_pos
+            command_joint_vel = self.ref_joint_vel
+            action_for_history = self.action
+
+        command_root = self.matrix_from_quat(ref_anchor_ori_w)
+        command_vec = np.concatenate(
+            (
+                command_joint_pos.squeeze(0),
+                command_joint_vel.squeeze(0),
+                # ref_anchor_pos_w,
+                # command_root[:, :2].reshape(-1),
+            ),
+            dtype=np.float32,
+        )
+
+        mimic_obs_buf = np.concatenate(
+            (
+                command_vec,
+                motion_anchor_ori_b[:, :2].reshape(-1),
+                ang_vel,
+                qj,
+                dqj[self.mj2lab],
+                np.asarray(action_for_history, dtype=np.float32).reshape(-1),
+            ),
+            axis=-1,
+            dtype=np.float32,
+        )
+        if mimic_obs_buf.shape[0] != self.num_obs:
+            raise RuntimeError(f"Observation length mismatch. Expected {self.num_obs}, got {mimic_obs_buf.shape[0]}.")
+        
+        if torch is not None:
+            mimic_obs_tensor = torch.from_numpy(mimic_obs_buf).unsqueeze(0).cpu().numpy()
+        else:
+            mimic_obs_tensor = np.expand_dims(mimic_obs_buf, axis=0)
+        observation = {}
+
+        # obs0 是网络观测，obs1 是当前时间步，用于输出参考动作信息
+        observation[self.input_name[0]] = mimic_obs_tensor
+        time_index = max(self.counter_step - self.warm_start_steps, 0)
+        
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if self.motion_length is not None and self.motion_length > 0:
+                time_index = min(time_index, self.motion_length - 1)
+
+        observation[self.input_name[1]] = np.array([[time_index]], dtype=np.float32)
+        outputs_result = self.ort_session.run(None, observation)
+        (
+            self.action,
+            self.ref_joint_pos,
+            self.ref_joint_vel,
+            self.ref_body_pos_w,
+            self.ref_body_quat_w,
+            self.ref_body_lin_vel_w,
+            self.ref_body_ang_vel_w,
+        ) = outputs_result
+
+        if (
+            self.hold_final_reference
+            and self.motion_length is not None
+            and self.motion_length > 0
+        ):
+            if time_index == self.motion_length - 1 and not self._final_ref_cached:
+                self._cache_final_ref()
+            elif self.counter_step >= self.motion_length and self._final_ref_cached:
+                self._apply_final_ref()
+        target_dof_pos_mj = np.zeros(29)
+        target_dof_pos_mj_23dof = np.zeros(23)
+        if use_warmstart and blended_target is not None:
+            target_dof_pos_lab = blended_target
+            # Keep action history aligned with the inserted warm trajectory.
+            self.action = np.asarray(action_for_history, dtype=np.float32).reshape(1, -1)
+        else:
+            target_dof_pos_lab = self.action * self.action_scale_lab + self.default_angles_lab
+            if target_dof_pos_lab.ndim > 1:
+                target_dof_pos_lab = np.squeeze(target_dof_pos_lab, axis=0)
+
+        if self.warm_start_steps > 0:
+            self._warmup_inference_counter += 1
+            if self._warmup_inference_counter <= self.warm_start_steps:
+                blend = self._warmup_inference_counter / self.warm_start_steps
+                if not use_warmstart:
+                    target_dof_pos_lab = (1.0 - blend) * self._warm_start_from_lab + blend * self._warm_start_to_lab
+
+        target_dof_pos_mj_23dof[self.mj2lab] = target_dof_pos_lab
+        target_dof_pos_mj[self.locked_joint_map] = target_dof_pos_mj_23dof
+
+        # Set joint commands exactly like C++
+        for i in range(self.motor_nums):
+            # C++: robot_data_->q_d_(35 - motor_num_ + i)
+            joint_idx = 35 - self.motor_nums + i
+            self.robot_data_.q_d_[joint_idx] = target_dof_pos_mj[i]
+            self.robot_data_.q_dot_d_[joint_idx] = 0.0
+            self.robot_data_.tau_d_[joint_idx] = 0.0
+
+        # update motion phase
+        self.counter_step += 1
+
+    def _cache_final_ref(self):
+        if not self.hold_final_reference:
+            return
+        self._final_ref_cached = True
+        self._final_ref_joint_pos = np.array(self.ref_joint_pos, copy=True)
+        self._final_ref_joint_vel = np.array(self.ref_joint_vel, copy=True)
+        self._final_ref_body_pos_w = np.array(self.ref_body_pos_w, copy=True)
+        self._final_ref_body_quat_w = np.array(self.ref_body_quat_w, copy=True)
+        self._final_ref_body_lin_vel_w = np.array(self.ref_body_lin_vel_w, copy=True)
+        self._final_ref_body_ang_vel_w = np.array(self.ref_body_ang_vel_w, copy=True)
+
+    def _apply_final_ref(self):
+        if not self.hold_final_reference or not self._final_ref_cached:
+            return
+        self.ref_joint_pos = np.array(self._final_ref_joint_pos, copy=True)
+        self.ref_joint_vel = np.array(self._final_ref_joint_vel, copy=True)
+        self.ref_body_pos_w = np.array(self._final_ref_body_pos_w, copy=True)
+        self.ref_body_quat_w = np.array(self._final_ref_body_quat_w, copy=True)
+        self.ref_body_lin_vel_w = np.array(self._final_ref_body_lin_vel_w, copy=True)
+        self.ref_body_ang_vel_w = np.array(self._final_ref_body_ang_vel_w, copy=True)
+
+    def run(self, flag: ControlFlag):
+        if int(self.robot_data_.time_now_ / self.physical_dt) % self.decimation_ == 0:
+            current_time = time.perf_counter()
+            print(f"Inference hz: {1/(current_time - self.last_run_time)}")
+            self.last_run_time = current_time
+            self.inner_run()
+        self.set_kp_kd()
+    def set_kp_kd(self):
+        # Set kp/kd gains
+        self.robot_data_.joint_kp_p_[:self.motor_nums] = self.kps
+        self.robot_data_.joint_kd_p_[:self.motor_nums] = self.kds
+    def on_exit(self):
+        self.action = np.zeros(self.num_actions, dtype=np.float32)
+        # self.action_buf = np.zeros(23 * self.history_length, dtype=np.float32)
+        self.ref_motion_phase = 0.
+        # self.ref_motion_phase_buf = np.zeros(1 * self.history_length, dtype=np.float32)
+        self.motion_time = 0
+        self.counter_step = 0
+        self._final_ref_cached = False
+        
+        print("exited")
+
+    
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDMIMIC":
+            return FSMStateName.BEYONDMIMIC
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        else:
+            return None  # 无状态转换
+
+    def _get_ref_anchor_pose(self):
+        ref_pos = self.ref_body_pos_w[:, self.anchor_body_index].squeeze(0)
+        ref_quat = self.ref_body_quat_w[:, self.anchor_body_index].squeeze(0)
+        return ref_pos.astype(np.float32), ref_quat.astype(np.float32)
+
+    def _get_current_joint_pos_lab(self) -> np.ndarray:
+        try:
+            current_q = self.robot_data_.get_joint_pos()
+            current_q = current_q[self.locked_joint_map]
+            current_q_lab = current_q[self.mj2lab]
+            return current_q_lab.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read current joint pose: {exc}")
+            return np.array(self.default_angles_lab, copy=True)
+
+    def _get_onnx_first_frame_lab(self) -> np.ndarray:
+        try:
+            action = self.action
+            if action is None:
+                raise RuntimeError("ONNX action output is None")
+            if action.ndim > 1:
+                action = np.squeeze(action, axis=0)
+            first_frame = action * self.action_scale_lab + self.default_angles_lab
+            return first_frame.astype(np.float32)
+        except Exception as exc:
+            print(f"[FSMStateBeyondMimic] Failed to read ONNX first frame: {exc}")
+            return np.array(self.default_angles_lab, copy=True)

Deploy_Tienkung/policy/stop/config/stop.yaml

@@ -0,0 +1,37 @@
+
+
+motor_num: 29        # 电机数量
+actions_size: 12     # action的大小
+dt: 0.002
+xsense_data_roll_offset: 0.0
+zero_pos_offset: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          3.207, 3.207, 3.207, #
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3, 
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+
+kp_pos: 
+  [1000, 2000, 2000, 1000, 50, 50, #left legs
+  1000, 2000, 2000, 1000, 50, 50,
+  1500.0, 1500.0, 1000.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0
+  ]  
+
+kd_pos: 
+  [30, 70, 70, 30, 3, 1, #left legs
+    30, 70, 70, 30, 3, 1, # right legs
+    70.0, 70.0, 50.0,  # 腰
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0,
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0
+  ]  
+

Deploy_Tienkung/policy/stop/fsm_stop.py

@@ -0,0 +1,90 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+
+import numpy as np
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+import yaml
+import os
+
+
+class FSMStateStop(FSMState):
+    """停止状态实现 - 与C++版本完全一致"""
+
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.STOP
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "stop.yaml")
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+
+        try:
+            self.action_num_ = policy_config["actions_size"]
+            self.motor_num_ = policy_config["motor_num"]
+
+            # Initialize vectors
+            self.hold_position_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+
+            # Load kp and kd gains from config
+            for i in range(self.motor_num_):
+                self.kp_pos_[i] = policy_config["kp_pos"][i]
+                self.kd_pos_[i] = policy_config["kd_pos"][i]
+
+        except Exception as e:
+            print(f"[FSMStateStop] YAML load error: {e}")
+            # Set default values like C++
+            self.action_num_ = 12
+            self.motor_num_ = 29
+            self.hold_position_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+
+    def on_enter(self):
+        """进入停止状态 - 与C++版本完全一致"""
+        # Store the last motor positions as hold positions (equivalent to tail(motor_num_))
+        self.hold_position_ = self.robot_data_.q_a_[-self.motor_num_:].copy()
+        print("[FSMStateStop] Enter stop state")
+
+    def run(self, flag: ControlFlag):
+        """运行停止状态 - 与C++版本完全一致"""
+        if self.robot_data_ is None:
+            return
+        print(f"""[FSMStateStop] Holding position: {self.hold_position_}""")
+        # Enforce the hold position for every frame (equivalent to tail(motor_num_))
+        self.robot_data_.q_d_[-self.motor_num_:] = self.hold_position_
+        # Set desired joint velocities to zero
+        self.robot_data_.q_dot_d_[-self.motor_num_:] = 0.0
+        # Set desired torques to zero
+        self.robot_data_.tau_d_[-self.motor_num_:] = 0.0
+
+        # Set proportional and derivative gains
+        self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_pos_
+        self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_pos_
+
+    def on_exit(self):
+        """退出停止状态 - 与C++版本完全一致"""
+        self.hold_position_.fill(0.0)
+        print("[FSMStateStop] Exit stop position control state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None  # 无状态转换

Deploy_Tienkung/policy/walk_amp/config/walk_amp.yaml

@@ -0,0 +1,98 @@
+model_path: "policy.onnx" 
+motor_num: 29        # 电机数量
+actions_size: 23     # action的大小
+dt: 0.01
+warm_start_time: 0.0
+xsense_data_roll_offset: 0.0
+joint_names: [
+  hip_pitch_l_joint, hip_pitch_r_joint, waist_yaw_joint,
+  hip_roll_l_joint, hip_roll_r_joint, waist_roll_joint,
+  hip_yaw_l_joint, hip_yaw_r_joint, waist_pitch_joint,
+  knee_pitch_l_joint, knee_pitch_r_joint,
+  shoulder_pitch_l_joint, shoulder_pitch_r_joint,
+  ankle_pitch_l_joint, ankle_pitch_r_joint,
+  shoulder_roll_l_joint, shoulder_roll_r_joint,
+  ankle_roll_l_joint, ankle_roll_r_joint,
+  shoulder_yaw_l_joint, shoulder_yaw_r_joint,
+  elbow_pitch_l_joint, elbow_pitch_r_joint
+]
+zero_pos_offset: [
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0,
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
+      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+    ]
+ct_scale: [
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          2.1, 2.1, 3.207, 2.673, 2.6, 2.6, 
+          3.207, 3.207, 3.207, #TODO:待更新 pry
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3, 
+          3.207, 2.28, 5.89, 5.89, 3.35, 2.3, 2.3
+    ]
+control:
+  # gait_cycle_period: 0.9
+  action_scale: 0.25               # 动作缩放比例，可写标量或 23 维数组
+  decimation: 1                   # 策略下发频率控制
+
+gait:
+  gait_air_ratio_l: 0.38
+  gait_air_ratio_r: 0.38
+  gait_phase_offset_l: 0.38
+  gait_phase_offset_r: 0.88
+  gait_cycle: 0.85
+
+normalization:
+  clip_scales:
+    clip_observations: 100. #18.0mlp
+    clip_actions: 100. #18.0
+  obs_scales:
+    lin_vel: 1.0                  # 线速度缩放因子
+    ang_vel: 1.0                  # 角速度缩放因子
+    dof_pos: 1.0                  # 关节位置缩放因子
+    dof_vel: 1.0                  # 关节速度缩放因子
+
+size:
+  num_hist: 10                     # 历史观测帧数
+  observations_size: 84          # 单帧观测长度
+
+gains:
+  kp: [
+    300.0, 300.0, 400.0,
+    300.0, 300.0, 400.0,
+    150.0, 150.0, 400.0,
+    350.0, 350.0,
+    150.0, 150.0,
+    30.0, 30.0,
+    50.0, 50.0,
+    16.8, 16.8,
+    50.0, 50.0,
+    150.0, 150.0
+  ]
+  kd: [
+    10.0, 10.0, 5.0,
+    10.0, 10.0, 10.0,
+    5.0, 5.0, 10.0,
+    10.0, 10.0,
+    7.5, 7.5,
+    2.5, 2.5,
+    2.5, 2.5,
+    1.4, 1.4,
+    2.5, 2.5,
+    5.0, 5.0
+  ]
+
+
+init_state:
+  default_joint_angles: [
+    -0.15, -0.15, 0.0,
+    0.0, 0.0, 0.0,
+    -0.0, -0.0, 0.0,
+    0.3, 0.3,
+    0.2, 0.2,
+    -0.15, -0.15,
+    0.1, -0.1,
+    0.0, 0.0,
+    0.0, 0.0,
+    -0.5, -0.5
+  ]

Deploy_Tienkung/policy/walk_amp/fsm_walkamp.py

@@ -0,0 +1,454 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+
+import numpy as np
+import onnxruntime as ort
+
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+from common.BasicFunction import clip_vector, gait_phase
+import os
+import yaml
+from scipy.spatial.transform import Rotation
+
+class FSMStateWALKAMP(FSMState):
+    """WALKAMP策略状态实现"""
+
+    def _reset_internal_state(self):
+        """把所有随时间变化的内部状态重置成初始值"""
+
+        # 1) 清空 obs / hist / actions
+        self.observations_.fill(0.0)
+        self.proprio_hist_buf_.fill(0.0)
+        self.last_actions_.fill(0.0)
+        self.actions_.fill(0.0)
+
+        # 2) 标志位重置
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self.is_first_step_ = True
+
+        # 3) 期望关节 / 期望速度 / 力矩重置为“初始姿态”
+        # 你已经有 self.joint_pos_array（mj 顺序，长度 len(self.joint_xml)）
+        base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+        # 期望角 = 初始角
+        self.robot_data_.q_d_[base:base + len(self.joint_xml)] = self.joint_pos_array
+        # 期望速度 = 0
+        self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+        # 期望力矩 = 0（位置控制）
+        self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+    def __init__(self,
+                 robot_data: RobotData,
+                 config_path: str | None = None,
+                 base_dir: str | None = None,
+                 log_prefix: str = "FSMStateWALKAMP"):
+        super().__init__(robot_data)
+        self.log_prefix = log_prefix
+
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        if config_path is None:
+            config_path = os.path.join(current_dir, "config", "walk_amp.yaml")
+        config_path = os.path.abspath(config_path)
+        if base_dir is None:
+            base_dir = os.path.dirname(os.path.dirname(config_path))
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+        # Load configuration exactly like C++
+        self.action_num_ = policy_config.get('actions_size')
+        self.motor_num_ = policy_config.get('motor_num')
+        self.dt_ = policy_config.get('dt')
+
+        # Size configuration
+        size_config = policy_config.get('size', {})
+        self.num_hist_ = size_config.get('num_hist')
+        self.obs_size_ = size_config.get('observations_size')
+
+        # Control configuration
+        control_config = policy_config.get('control', {})
+        self.action_scale_ = control_config.get('action_scale')
+        # self.gait_cycle_period_ = control_config.get('gait_cycle_period', 1.0)
+        self.decimation_ = control_config.get('decimation')
+        self.warm_start_time_ = control_config.get(
+            'warm_start_time',
+            policy_config.get('warm_start_time', 0.3),
+        )
+
+        # Normalization configuration
+        norm_config = policy_config.get('normalization', {})
+        clip_config = norm_config.get('clip_scales', {})
+        obs_config = norm_config.get('obs_scales', {})
+
+        self.clip_obs_ = clip_config.get('clip_observations', 100.0)
+        self.clip_act_ = clip_config.get('clip_actions', 100.0)
+        self.lin_vel_scale_ = obs_config.get('lin_vel')
+        self.ang_vel_scale_ = obs_config.get('ang_vel')
+        self.dof_pos_scale_ = obs_config.get('dof_pos')
+        self.dof_vel_scale_ = obs_config.get('dof_vel')
+
+
+        # Initialize buffers and actions
+        self.observations_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.proprio_hist_buf_ = np.zeros(self.obs_size_ * self.num_hist_, dtype=np.float32)
+        self.last_actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self.actions_ = np.zeros(self.action_num_, dtype=np.float32)
+        self._warm_start_pose = np.zeros(self.motor_num_, dtype=np.float32)
+
+
+        # Flags matching C++
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        # self.phase_locked = False
+        self.timer_gait_ = 0.0
+        gait_config = policy_config.get('gait', {})
+        self.gait_cycle = gait_config.get('gait_cycle', 0.85)
+        self.left_phase_ratio = gait_config.get('gait_air_ratio_l', 0.38)
+        self.right_phase_ratio = gait_config.get('gait_air_ratio_r', 0.38)
+        self.left_theta_offset = gait_config.get('gait_phase_offset_l', 0.38)
+        self.right_theta_offset = gait_config.get('gait_phase_offset_r', 0.88)
+        
+        self.is_first_step_ = True
+        step = (self.decimation_ if self.decimation_ else 1) * self.dt_
+        if self.warm_start_time_ > 0 and step > 0:
+            self._warm_start_steps = max(1, int(self.warm_start_time_ / step))
+        else:
+            self._warm_start_steps = 0
+        self._warmup_inference_counter = 0
+
+
+        # Initialize ONNX session
+        self.model_path = os.path.join(base_dir, "model", policy_config["model_path"])
+        self._init_onnx_session()
+
+        self.joint_seq = None
+        self.joint_pos_array_seq = None
+        self.action_scale = None
+        self.stiffness_array_seq = None
+        self.damping_array_seq = None
+        
+        joint_names = policy_config.get('joint_names')
+        if joint_names is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'joint_names' in walk_amp.yaml")
+
+        self.joint_seq = list(joint_names)
+
+        if self.action_scale_ is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'control.action_scale' in walk_amp.yaml")
+
+        if np.isscalar(self.action_scale_):
+            self.action_scale = np.full(len(self.joint_seq), float(self.action_scale_), dtype=np.float32)
+        else:
+            self.action_scale = np.array(self.action_scale_, dtype=np.float32)
+
+        if len(self.action_scale) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] control.action_scale length {len(self.action_scale)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        init_state_config = policy_config.get('init_state', {})
+        default_joint_angles = init_state_config.get('default_joint_angles')
+        if default_joint_angles is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'init_state.default_joint_angles' in walk_amp.yaml")
+
+        self.joint_pos_array_seq = np.array(default_joint_angles, dtype=np.float32)
+        if len(self.joint_pos_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] init_state.default_joint_angles length {len(self.joint_pos_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+
+        gains_config = policy_config.get('gains', {})
+        kp_values = gains_config.get('kp')
+        kd_values = gains_config.get('kd')
+        if kp_values is None or kd_values is None:
+            raise ValueError("[FSMStateWALKAMP] Missing 'gains.kp' or 'gains.kd' in walk_amp.yaml")
+
+        self.stiffness_array_seq = np.array(kp_values, dtype=np.float32)
+        self.damping_array_seq = np.array(kd_values, dtype=np.float32)
+
+        if len(self.stiffness_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] gains.kp length {len(self.stiffness_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        if len(self.damping_array_seq) != len(self.joint_seq):
+            raise ValueError(
+                f"[FSMStateWALKAMP] gains.kd length {len(self.damping_array_seq)} does not match joint count {len(self.joint_seq)}"
+            )
+        # # 设置从序列到实验室顺序的映射
+        self.joint_xml = [
+            "hip_pitch_l_joint", "hip_roll_l_joint", "hip_yaw_l_joint",
+            "knee_pitch_l_joint", "ankle_pitch_l_joint", "ankle_roll_l_joint",
+            "hip_pitch_r_joint", "hip_roll_r_joint", "hip_yaw_r_joint",
+            "knee_pitch_r_joint", "ankle_pitch_r_joint", "ankle_roll_r_joint",
+            "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
+            "shoulder_pitch_l_joint", "shoulder_roll_l_joint", "shoulder_yaw_l_joint",
+            "elbow_pitch_l_joint", "elbow_yaw_l_joint", "wrist_pitch_l_joint", "wrist_roll_l_joint",
+            "shoulder_pitch_r_joint", "shoulder_roll_r_joint", "shoulder_yaw_r_joint",
+            "elbow_pitch_r_joint", "elbow_yaw_r_joint", "wrist_pitch_r_joint", "wrist_roll_r_joint",
+        ]
+
+        # 从MjXUML顺序映射到实验室顺序
+        # self.mj2lab = np.array([self.joint_xml.index(joint) for joint in self.joint_seq])
+        self.lab2mj = []
+        for name in self.joint_seq:
+            if name not in self.joint_xml:
+                raise ValueError(f"[FSMStateWALKAMP] joint '{name}' from walk_amp.yaml not found in joint_xml!")
+            self.lab2mj.append(self.joint_xml.index(name))
+        self.lab2mj = np.array(self.lab2mj, dtype=int)
+
+        # 从实验室顺序映射到MjXUML顺序
+        # ====== 把 23 个 lab 关节 scatter 到 29 个 xml 里，多的 6 个保持默认 ======
+        n_mj = len(self.joint_xml)
+
+        # 29 长度，mujoco XML 顺序，先全 0 或者你想要的默认值
+        self.joint_pos_array = np.zeros(n_mj, dtype=np.float32)
+        self.stiffness_array = np.zeros(n_mj, dtype=np.float32)
+        self.damping_array = np.zeros(n_mj, dtype=np.float32)
+
+        # joint_pos_array_seq / stiffness_array_seq / damping_array_seq 是 23 长度，lab 顺序
+        for lab_idx, mj_idx in enumerate(self.lab2mj):
+            self.joint_pos_array[mj_idx] = self.joint_pos_array_seq[lab_idx]
+            self.stiffness_array[mj_idx] = self.stiffness_array_seq[lab_idx]
+            self.damping_array[mj_idx] = self.damping_array_seq[lab_idx]
+
+
+        # 设置其他参数
+        self.kps_lab = self.stiffness_array_seq
+        self.kds_lab = self.damping_array_seq
+        self.default_angles_lab = self.joint_pos_array_seq
+        self.action_scale_lab = self.action_scale
+
+
+        self.filtered_x_speed = 0
+
+    def _init_onnx_session(self):
+        """初始化ONNX推理会话"""
+        try:
+            # 配置SessionOptions
+            options = ort.SessionOptions()
+
+            # 启用图优化，使用所有可用的优化（包括算子融合等）
+            options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+
+            # 设置执行模式（可选，默认执行模式是顺序执行，但图优化会改变计算图）
+            # 设置线程数（根据CPU核心数调整）
+            # 建议设置为CPU物理核心数（非超线程数），因为超线程可能不会带来线性提升
+            options.intra_op_num_threads = 1  # 设置计算图中的运算符内部并行线程数
+            options.inter_op_num_threads = 1  # 设置多个运算符之间的并行线程数（如果模型有多个分支）
+
+            # 启用内存优化（避免重复分配内存）
+            options.enable_mem_pattern = False  # 对于固定输入大小，可以设为False以避免内存规划的开销
+            options.enable_mem_reuse = True # 启用内存重用机制
+
+            self.ort_session_ = ort.InferenceSession(self.model_path, options, providers=['CPUExecutionProvider'])
+            
+            print(f"[{self.log_prefix}-ONNX] ONNX model loaded successfully: {self.model_path}")
+        except Exception as e:
+            print(f"[{self.log_prefix}] Failed to load ONNX model: {e}")
+            self.ort_session_ = None
+
+    def on_enter(self):
+        """进入WALKAMP状态"""
+        self._reset_internal_state()
+        print(f"[{self.log_prefix}] enter")
+        self.is_first_obs_ = True
+        self.is_first_action_ = True
+        self._warmup_inference_counter = 0
+        self.timer_gait_ = 0.0
+        if self.robot_data_ is not None:
+            try:
+                self._warm_start_pose = self.robot_data_.get_joint_pos().copy()
+            except Exception:
+                self._warm_start_pose.fill(0.0)
+        else:
+            self._warm_start_pose.fill(0.0)
+
+    def run(self, flag: ControlFlag):
+        """运行WALKAMP状态 - 与C++版本完全一致"""
+        print(f"[{self.log_prefix}] run")
+        # Only run policy inference every decimation_ steps
+        gait = gait_phase(
+            self.timer_gait_,
+            self.gait_cycle,
+            self.left_theta_offset,
+            self.right_theta_offset,
+            self.left_phase_ratio,
+            self.right_phase_ratio,
+        ).astype(np.float32)
+
+        if int(self.robot_data_.time_now_ / self.dt_) % self.decimation_ == 0:
+
+            # print(f"[FSMStateWALKAMP] Gait phase: {gait}")
+            self.compute_observation(flag,gait)
+            self.compute_actions()
+
+            # lab 顺序目标角 23 维
+            target_dof_pos_lab = self.actions_ * self.action_scale_lab + self.default_angles_lab
+
+            # 拿一份当前 mj 顺序的关节角（或你原来用的 default 也行）
+            target_dof_pos_mj = self.robot_data_.get_joint_pos().copy()
+
+            # 只更新 23 个受控 DOF
+            target_dof_pos_mj[self.lab2mj] = target_dof_pos_lab
+            commanded_pos = target_dof_pos_mj
+            if self._warm_start_steps > 0 and self._warmup_inference_counter < self._warm_start_steps:
+                self._warmup_inference_counter += 1
+                blend = self._warmup_inference_counter / float(self._warm_start_steps)
+                commanded_pos = (1.0 - blend) * self._warm_start_pose + blend * target_dof_pos_mj
+
+            base = self.robot_data_.q_d_.shape[0] - self.motor_num_
+            self.robot_data_.q_d_[base:base + len(self.joint_xml)] = commanded_pos
+
+            self.robot_data_.q_dot_d_[base:base + len(self.joint_xml)] = 0.0
+            self.robot_data_.tau_d_[base:base + len(self.joint_xml)] = 0.0
+
+            self.last_actions_[:] = self.actions_
+
+
+        self.timer_gait_ += self.dt_
+        self.robot_data_.joint_kp_p_[:len(self.joint_xml)] = self.stiffness_array
+        self.robot_data_.joint_kd_p_[:len(self.joint_xml)] = self.damping_array
+
+    def compute_observation(self, flag: ControlFlag, gait):
+        roll, pitch, yaw = (
+                        float(self.robot_data_.imu_data_[2]),
+                        float(self.robot_data_.imu_data_[1]),
+                        float(self.robot_data_.imu_data_[0]),
+                    )
+        quat_wxyz = self.euler_to_quaternion_scipy(roll, pitch, yaw)
+        q_xyzw    = np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]], dtype=np.float32)
+        gravity_init   = self.quat_rotate_inverse_numpy(q_xyzw, np.array([0.,0.,-1.], dtype=np.float32))
+        
+
+        x_speed_command, y_speed_command, yaw_speed_command = self.robot_data_.get_walk_cmd()
+        new_filtered_x_speed = 1 * x_speed_command + (1 - 1) * self.filtered_x_speed
+        change = new_filtered_x_speed - self.filtered_x_speed
+        change = np.clip(change, -0.005, 0.005)
+        self.filtered_x_speed = self.filtered_x_speed + change
+        command = np.concatenate([
+            np.array([
+                x_speed_command,
+                y_speed_command,
+                yaw_speed_command,
+            ], dtype=np.float32),
+        ])
+        print(f'Input command: {command}')
+
+        gyro = np.array([
+            self.robot_data_.imu_data_[3],
+            self.robot_data_.imu_data_[4],
+            self.robot_data_.imu_data_[5]
+        ], dtype=np.float32) * self.ang_vel_scale_
+
+        q_mj = self.robot_data_.get_joint_pos()
+        qdot_mj = self.robot_data_.get_joint_vel()
+
+
+
+        ang_vel = self.robot_data_.get_angular_velocity()
+        q_mj = self.robot_data_.get_joint_pos()   # mj 顺序，长度 29
+        dq_mj = self.robot_data_.get_joint_vel()
+
+        # 只取 23 个受控关节，变成 lab 顺序
+        qj = q_mj[self.lab2mj]
+        dqj = dq_mj[self.lab2mj]
+
+        qj = qj - self.default_angles_lab
+
+
+        # Observation = ang_vel(3) + gravity(3) + command(9) + q(23) + dq(23) + action(23) = 84
+        proprio = np.concatenate([
+            ang_vel ,              # 3 elements
+            gravity_init,
+            command,
+            qj,
+            dqj,
+            self.last_actions_,
+            gait
+        ])
+
+        # History buffer management exactly like C++
+        if self.is_first_obs_:
+            for i in range(self.num_hist_):
+                start_idx = i * self.obs_size_
+                end_idx = start_idx + self.obs_size_
+                self.proprio_hist_buf_[start_idx:end_idx] = proprio
+            self.is_first_obs_ = False
+        else:
+            # Shift history: head((num_hist-1)*obs_size) = tail((num_hist-1)*obs_size)
+            shift_size = (self.num_hist_ - 1) * self.obs_size_
+            self.proprio_hist_buf_[:shift_size] = self.proprio_hist_buf_[self.obs_size_:]
+            self.proprio_hist_buf_[shift_size:] = proprio
+
+        # Clip observations exactly like C++
+        self.observations_ = np.clip(self.proprio_hist_buf_, -self.clip_obs_, self.clip_obs_)
+
+
+    @staticmethod
+    def euler_to_quaternion_scipy(roll, pitch, yaw, degrees=False):
+        r = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=degrees)
+        q_xyzw = r.as_quat()
+        return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]], dtype=np.float32)
+
+    @staticmethod
+    def quat_rotate_inverse_numpy(q_xyzw, v):
+        q_w = q_xyzw[3]
+        q_v = q_xyzw[:3]
+        a = v * (2.0 * q_w * q_w - 1.0)
+        b = np.cross(q_v, v) * (2.0 * q_w)
+        c = q_v * (2.0 * np.dot(q_v, v))
+        return a - b + c
+    def compute_actions(self):
+        if self.ort_session_ is None:
+            return
+
+        try:
+            # Prepare input tensor
+            input_data = self.observations_.reshape(1, -1).astype(np.float32)
+
+            # ONNX inference
+            input_name = self.ort_session_.get_inputs()[0].name
+            outputs = self.ort_session_.run(None, {input_name: input_data})
+
+            # Extract and clip actions exactly like C++
+            output_data = outputs[0][0]
+            for i in range(self.action_num_):
+                self.actions_[i] = np.clip(output_data[i], -self.clip_act_, self.clip_act_)
+
+            if self.is_first_action_:
+                print(f"[{self.log_prefix}-ONNX] First Observation:")
+                for i in range(self.obs_size_):
+                    print(f"{self.observations_[i]:.6f} ", end="")
+                print()
+                self.is_first_action_ = False
+
+        except Exception as e:
+            print(f"[{self.log_prefix}] ONNX Runtime inference error: {e}")
+
+    def on_exit(self):
+        """退出WALKAMP状态"""
+        print(f"[{self.log_prefix}] exit")
+        # 关掉 obs 日志文件
+        if getattr(self, "obs_log_file", None) is not None:
+            try:
+                self.obs_log_file.flush()
+                self.obs_log_file.close()
+                print(f"[{self.log_prefix}] obs log saved to {self.obs_log_path}")
+            except Exception as e:
+                print(f"[{self.log_prefix}] failed to close obs log: {e}")
+            self.obs_log_file = None
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        else:
+            return None  # 无状态转换

Deploy_Tienkung/policy/zero/config/zero.yaml

@@ -0,0 +1,39 @@
+motor_num: 29        # 电机数量
+actions_size: 12     # action的大小
+
+zero_positions: [
+    -0.25, 0, 0, 0.5, -0.25, 0.0,   # 左腿
+    -0.25, 0, 0, 0.5, -0.25, 0.0,   # 右腿
+    0.0, 0.0, 0.0,                   # 腰
+    0.0, 0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 左臂
+    0.0, -0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 右臂
+    ]  
+    
+zero_positions_height: [
+    -0.4, 0.0, 0, 0.8, -0.4, 0.0,   # 左腿
+    -0.4, 0.0, 0, 0.8, -0.4, 0.0,   # 右腿
+    0.05, 0.08, 0.11,                   # 腰
+    0.0, 0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 左臂
+    0.0, -0.3, 0.0, -0.3, 0.0, 0.0, 0.0, # 右臂
+    ]  
+
+kp_pos: 
+  [1000, 2000, 2000, 1000, 50, 50, #left legs
+  1000, 2000, 2000, 1000, 50, 50,
+  1500.0, 1500.0, 1000.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0,
+  200.0, 200.0, 200.0, 200.0, 80.0, 80.0, 80.0
+  ]  
+
+kd_pos: 
+  [30, 70, 70, 30, 3, 1, #left legs
+    30, 70, 70, 30, 3, 1, # right legs
+    70.0, 70.0, 50.0,  # 腰
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0,
+    15, 15, 15.0, 15.0, 5.0, 5.0, 5.0
+  ]  
+
+
+
+interp_step: 0.001 #  控制插值速度
+interp_max: 0.9

Deploy_Tienkung/policy/zero/fsm_zero.py

@@ -0,0 +1,87 @@
+"""
+FSM State Implementations
+Concrete implementations of different FSM states
+"""
+import numpy as np
+from FSM.fsm_base import FSMState, FSMStateName
+from common.joystick import ControlFlag
+from common.robot_data import RobotData
+import os
+import yaml
+
+
+class FSMStateZero(FSMState):
+    """零位状态实现（完整C++逻辑移植）"""
+    def __init__(self, robot_data: RobotData):
+        super().__init__(robot_data)
+        self.current_state_name = FSMStateName.ZERO
+        self.q_factor_ = 0.0
+        # 获取包路径
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        config_path = os.path.join(current_dir, "config", "zero.yaml")
+        with open(config_path, 'r') as f:
+            policy_config = yaml.safe_load(f)
+
+        try:
+            self.action_num_ = policy_config["actions_size"]
+            self.motor_num_ = policy_config["motor_num"]
+            self.zero_positions_ = np.array(policy_config["zero_positions"], dtype=float)
+            self.zero_positions_height_ = np.array(policy_config["zero_positions_height"], dtype=float)
+            self.kp_pos_ = np.array(policy_config["kp_pos"], dtype=float)
+            self.kd_pos_ = np.array(policy_config["kd_pos"], dtype=float)
+            self.interp_step_ = float(policy_config["interp_step"])
+            self.interp_max_ = float(policy_config["interp_max"])
+        except Exception as e:
+            print(f"[FSMStateZero] YAML load error: {e}")
+            self.action_num_ = 12
+            self.motor_num_ = 29
+            self.zero_positions_ = np.zeros(self.motor_num_)
+            self.zero_positions_height_ = np.zeros(self.motor_num_)
+            self.kp_pos_ = np.zeros(self.motor_num_)
+            self.kd_pos_ = np.zeros(self.motor_num_)
+            self.interp_step_ = 0.00002
+            self.interp_max_ = 0.9
+        self.zero_positions = np.zeros(self.motor_num_)
+
+    def on_enter(self):
+        print("[FSMStateZero] Enter zero state")
+        self.q_factor_ = 0.0
+
+    def run(self, flag: ControlFlag):
+        if self.robot_data_ is None:
+            return
+        q_est = self.robot_data_.q_a_[-self.motor_num_:]  # numpy数组切片
+        if getattr(flag, 'height_control', False):
+            self.zero_positions = self.zero_positions_height_
+        else:
+            self.zero_positions = self.zero_positions_
+        if self.q_factor_ < self.interp_max_:
+            pos_cmd = (1.0 - self.q_factor_) * q_est + self.q_factor_ * self.zero_positions
+            self.q_factor_ = min(self.q_factor_ + self.interp_step_, self.interp_max_)
+        else:
+            pos_cmd = self.zero_positions
+        self.robot_data_.q_d_[-self.motor_num_:] = pos_cmd
+        self.robot_data_.q_dot_d_[-self.motor_num_:] = 0
+        self.robot_data_.tau_d_[-self.motor_num_:] = 0
+        self.robot_data_.joint_kp_p_[:self.motor_num_] = self.kp_pos_
+        self.robot_data_.joint_kd_p_[:self.motor_num_] = self.kd_pos_
+
+    def on_exit(self):
+        print("[FSMStateZero] Exit zero position control state")
+
+    def check_transition(self, flag: ControlFlag) -> FSMStateName:
+        """检查状态转换"""
+        if flag.fsm_state_command == "gotoSTOP":
+            return FSMStateName.STOP
+        elif flag.fsm_state_command == "gotoWALKAMP":
+            return FSMStateName.WALKAMP
+        elif flag.fsm_state_command == "gotoMYPOLICY":
+            return FSMStateName.MYPOLICY
+        elif flag.fsm_state_command == "gotoXSIMRUN":
+            return FSMStateName.XSIMRUN
+        elif flag.fsm_state_command == "gotoZERO":
+            return FSMStateName.ZERO
+        elif flag.fsm_state_command == "gotoBEYONDZERO":
+            return FSMStateName.BEYONDZERO
+        else:
+            return None  # 无状态转换