add deploy code on g1

deploy_real/ReadMe.md

@@ -0,0 +1,7 @@
+# Deploy on G1
+
+## Usage
+Here is a usage example, for more information, check [unitree_rl_gym](https://github.com/unitreerobotics/unitree_rl_gym)
+```bash
+python deploy_real/deploy_real.py enp3s0 g1_23.yaml
+```

deploy_real/common/command_helper.py

@@ -0,0 +1,60 @@
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as LowCmdGo
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as LowCmdHG
+from typing import Union
+
+class MotorMode:
+    PR = 0  # Series Control for Pitch/Roll Joints
+    AB = 1  # Parallel Control for A/B Joints
+
+
+def create_damping_cmd(cmd: Union[LowCmdGo, LowCmdHG]):
+    size = len(cmd.motor_cmd)
+    for i in range(size):
+        cmd.motor_cmd[i].q = 0
+        cmd.motor_cmd[i].qd = 0
+        cmd.motor_cmd[i].kp = 0
+        cmd.motor_cmd[i].kd = 8
+        cmd.motor_cmd[i].tau = 0
+
+
+def create_zero_cmd(cmd: Union[LowCmdGo, LowCmdHG]):
+    size = len(cmd.motor_cmd)
+    for i in range(size):
+        cmd.motor_cmd[i].q = 0
+        cmd.motor_cmd[i].qd = 0
+        cmd.motor_cmd[i].kp = 0
+        cmd.motor_cmd[i].kd = 0
+        cmd.motor_cmd[i].tau = 0
+
+
+def init_cmd_hg(cmd: LowCmdHG, mode_machine: int, mode_pr: int):
+    cmd.mode_machine = mode_machine
+    cmd.mode_pr = mode_pr
+    size = len(cmd.motor_cmd)
+    for i in range(size):
+        cmd.motor_cmd[i].mode = 1
+        cmd.motor_cmd[i].q = 0
+        cmd.motor_cmd[i].qd = 0
+        cmd.motor_cmd[i].kp = 0
+        cmd.motor_cmd[i].kd = 0
+        cmd.motor_cmd[i].tau = 0
+
+
+def init_cmd_go(cmd: LowCmdGo, weak_motor: list):
+    cmd.head[0] = 0xFE
+    cmd.head[1] = 0xEF
+    cmd.level_flag = 0xFF
+    cmd.gpio = 0
+    PosStopF = 2.146e9
+    VelStopF = 16000.0
+    size = len(cmd.motor_cmd)
+    for i in range(size):
+        if i in weak_motor:
+            cmd.motor_cmd[i].mode = 1
+        else:
+            cmd.motor_cmd[i].mode = 0x0A
+        cmd.motor_cmd[i].q = PosStopF
+        cmd.motor_cmd[i].qd = VelStopF
+        cmd.motor_cmd[i].kp = 0
+        cmd.motor_cmd[i].kd = 0
+        cmd.motor_cmd[i].tau = 0

deploy_real/common/motion_lib_helper.py

@@ -0,0 +1,19 @@
+import joblib
+
+def get_motion_len(motion_file_path: str):
+    """
+    Get the length of the motion from a motion file.
+
+    Args:
+        motion_file_path (str): Path to the motion file.
+
+    Returns:
+        int: Length of the motion.
+    """
+    motion_data = joblib.load(motion_file_path)
+    motion_data = motion_data[list(motion_data.keys())[0]]
+    fps = motion_data["fps"]
+    dt = 1.0 / fps
+    num_frames = motion_data["root_rot"].shape[0]
+    motion_len = dt * (num_frames - 1)
+    return motion_len

deploy_real/common/remote_controller.py

@@ -0,0 +1,39 @@
+import struct
+
+
+class KeyMap:
+    R1 = 0
+    L1 = 1
+    start = 2
+    select = 3
+    R2 = 4
+    L2 = 5
+    F1 = 6
+    F2 = 7
+    A = 8
+    B = 9
+    X = 10
+    Y = 11
+    up = 12
+    right = 13
+    down = 14
+    left = 15
+
+
+class RemoteController:
+    def __init__(self):
+        self.lx = 0
+        self.ly = 0
+        self.rx = 0
+        self.ry = 0
+        self.button = [0] * 16
+
+    def set(self, data):
+        # wireless_remote
+        keys = struct.unpack("H", data[2:4])[0]
+        for i in range(16):
+            self.button[i] = (keys & (1 << i)) >> i
+        self.lx = struct.unpack("f", data[4:8])[0]
+        self.rx = struct.unpack("f", data[8:12])[0]
+        self.ry = struct.unpack("f", data[12:16])[0]
+        self.ly = struct.unpack("f", data[20:24])[0]

deploy_real/common/rotation_helper.py

@@ -0,0 +1,25 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+def get_gravity_orientation(quaternion):
+    qw = quaternion[0]
+    qx = quaternion[1]
+    qy = quaternion[2]
+    qz = quaternion[3]
+
+    gravity_orientation = np.zeros(3)
+
+    gravity_orientation[0] = 2 * (-qz * qx + qw * qy)
+    gravity_orientation[1] = -2 * (qz * qy + qw * qx)
+    gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz)
+
+    return gravity_orientation
+
+
+def transform_imu_data(waist_yaw, waist_yaw_omega, imu_quat, imu_omega):
+    RzWaist = R.from_euler("z", waist_yaw).as_matrix()
+    R_torso = R.from_quat([imu_quat[1], imu_quat[2], imu_quat[3], imu_quat[0]]).as_matrix()
+    R_pelvis = np.dot(R_torso, RzWaist.T)
+    w = np.dot(RzWaist, imu_omega[0]) - np.array([0, 0, waist_yaw_omega])
+    return R.from_matrix(R_pelvis).as_quat()[[3, 0, 1, 2]], w

deploy_real/config.py

@@ -0,0 +1,45 @@
+import numpy as np
+import yaml
+
+class Config:
+    def __init__(self, file_path) -> None:
+        with open(file_path, "r") as f:
+            config = yaml.load(f, Loader=yaml.FullLoader)
+
+            self.control_dt = config["control_dt"]
+
+            self.msg_type = config["msg_type"]
+            self.imu_type = config["imu_type"]
+
+            self.weak_motor = []
+            if "weak_motor" in config:
+                self.weak_motor = config["weak_motor"]
+
+            self.lowcmd_topic = config["lowcmd_topic"]
+            self.lowstate_topic = config["lowstate_topic"]
+
+            self.policy_path = config["policy_path"]
+
+            self.leg_joint2motor_idx = config["leg_joint2motor_idx"]
+            self.kps = config["kps"]
+            self.kds = config["kds"]
+            self.default_angles = np.array(config["default_angles"], dtype=np.float32)
+
+            self.arm_waist_joint2motor_idx = config["arm_waist_joint2motor_idx"]
+            self.arm_waist_kps = config["arm_waist_kps"]
+            self.arm_waist_kds = config["arm_waist_kds"]
+            self.arm_waist_target = np.array(config["arm_waist_target"], dtype=np.float32)
+
+            self.ang_vel_scale = config["ang_vel_scale"]
+            self.dof_pos_scale = config["dof_pos_scale"]
+            self.dof_vel_scale = config["dof_vel_scale"]
+            self.action_scale = config["action_scale"]
+            self.cmd_scale = np.array(config["cmd_scale"], dtype=np.float32)
+            self.max_cmd = np.array(config["max_cmd"], dtype=np.float32)
+
+            self.num_actions = config["num_actions"]
+            self.num_obs = config["num_obs"]
+
+            self.frame_stack = config.get("frame_stack", 5)
+            self.motion_file = config["motion_file"]
+            self.clip_action_limit = config["clip_action_limit"]

deploy_real/configs/g1_23.yaml

@@ -0,0 +1,58 @@
+control_dt: 0.02
+
+msg_type: "hg"     # "hg" or "go"
+imu_type: "pelvis"    # "torso" or "pelvis"
+
+lowcmd_topic: "rt/lowcmd"
+lowstate_topic: "rt/lowstate"
+
+policy_path: "example/pretrained_horse_stance_punch/exported/model_33000.onnx"
+motion_file: "example/motion_data/Horse-stance_punch.pkl"
+
+leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 
+                      6, 7, 8, 9, 10, 11, 
+                      12, 13, 14, 
+                      15, 16, 17, 18, 
+                      22, 23, 24, 25,
+                    ]
+kps: [100, 100, 100, 150, 40, 40, 
+      100, 100, 100, 150, 40, 40, 
+      400, 400, 400, 
+      100, 100, 50, 50,
+      100, 100, 50, 50,
+    ]
+kds: [2, 2, 2, 4, 2, 2, 
+      2, 2, 2, 4, 2, 2, 
+      5, 5, 5,
+      2, 2, 2, 2,
+      2, 2, 2, 2,
+    ]
+default_angles: [-0.1,  0.0,  0.0,  0.3, -0.2, 0.0, 
+                 -0.1,  0.0,  0.0,  0.3, -0.2, 0.0,
+                  0.0,  0.0,  0.0,
+                  0.2, 0.2, 0.0, 0.9, 
+                  0.2, -0.2, 0.0, 0.9, 
+                ]
+
+arm_waist_joint2motor_idx: [19, 20, 21,
+                            26, 27, 28]
+
+arm_waist_kps: [20, 20, 20,
+                20, 20, 20,]
+
+arm_waist_kds: [1, 1, 1,
+                1, 1, 1]
+
+arm_waist_target: [ 0.0, 0.0, 0.0,
+                    0.0, 0.0, 0.0,]
+ang_vel_scale: 0.25
+dof_pos_scale: 1.0
+dof_vel_scale: 0.05
+action_scale: 0.25
+cmd_scale: [2.0, 2.0, 0.25]
+num_actions: 23
+num_obs: 76
+frame_stack: 5
+clip_action_limit: 100.0
+
+max_cmd: [1., 0., 1]