├── .gitignore ├── LICENSE ├── README.md ├── data └── g1 │ └── dance1_subject2_rev.csv ├── demo.sh ├── img ├── dance.gif ├── manual_ip.png └── sanity.gif ├── music ├── robot_merge2.wav └── robot_merge3.wav ├── run_check.py └── run_retarget.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # UV 98 | # Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | #uv.lock 102 | 103 | # poetry 104 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 105 | # This is especially recommended for binary packages to ensure reproducibility, and is more 106 | # commonly ignored for libraries. 107 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 108 | #poetry.lock 109 | 110 | # pdm 111 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 112 | #pdm.lock 113 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 114 | # in version control. 115 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 116 | .pdm.toml 117 | .pdm-python 118 | .pdm-build/ 119 | 120 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 121 | __pypackages__/ 122 | 123 | # Celery stuff 124 | celerybeat-schedule 125 | celerybeat.pid 126 | 127 | # SageMath parsed files 128 | *.sage.py 129 | 130 | # Environments 131 | .env 132 | .venv 133 | env/ 134 | venv/ 135 | ENV/ 136 | env.bak/ 137 | venv.bak/ 138 | 139 | # Spyder project settings 140 | .spyderproject 141 | .spyproject 142 | 143 | # Rope project settings 144 | .ropeproject 145 | 146 | # mkdocs documentation 147 | /site 148 | 149 | # mypy 150 | .mypy_cache/ 151 | .dmypy.json 152 | dmypy.json 153 | 154 | # Pyre type checker 155 | .pyre/ 156 | 157 | # pytype static type analyzer 158 | .pytype/ 159 | 160 | # Cython debug symbols 161 | cython_debug/ 162 | 163 | # PyCharm 164 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 165 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 166 | # and can be added to the global gitignore or merged into this file. For a more nuclear 167 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 168 | #.idea/ 169 | 170 | # PyPI configuration file 171 | .pypirc 172 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Chuan Li 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # unitree-retarget 2 | 3 | Retarget [mocap data](https://huggingface.co/datasets/unitreerobotics/LAFAN1_Retargeting_Dataset) to Unitree G1 4 | 5 | ## Installation 6 | 7 | Install `unitree_sdk2py`: 8 | ``` 9 | virtualenv -p /usr/bin/python venv-unitree 10 | . venv-unitree/bin/activate 11 | 12 | git clone https://github.com/unitreerobotics/unitree_sdk2_python.git 13 | cd unitree_sdk2_python 14 | git checkout 4eadcf6ba0cc822501ff2f1766be637c0fb7f229 15 | pip3 install -e . 16 | cd - 17 | 18 | git clone https://github.com/LambdaLabsML/unitree-retarget.git 19 | cd unitree-retarget 20 | ``` 21 | 22 | ## Usage 23 | 24 | __Step 1__: Turn on `G1` and put it into debug mode 25 | Make sure `G1` is secured on the harness. Then follow the "Development and debugging video" on [this page](https://support.unitree.com/home/en/G1_developer/quick_start) to put it under the debug mode (on your remote controller, do `R2+L2` then `L2+A`) 26 | 27 | 28 | __Step 2__: Connect your computer to unitree robot 29 | 30 | You can directly connect them using a CAT6 cable, or connect both of them to a router. In both cases, make sure manually set IPv4 address to `192.168.123.222` with mask `255.255.255.0`. This is because `G1` has static a IP `192.168.123.164`, and DHCP won't automatically put your computer under the same subnet. 31 | 32 | ![Image Description](img/manual_ip.png) 33 | 34 | 35 | As a sanity check, you should be able to: 36 | * Ping `G1` with `ping 192.168.123.164` 37 | * ssh into it with `ssh unitree@192.168.123.164` and password `123` 38 | 39 | __Step 3__: Sanity Check 40 | 41 | `python run_check.py` 42 | 43 | ![Demo](img/sanity.gif) 44 | 45 | 46 | __Step 4__: Retarget 47 | 48 | `python run_retarget.py --mocap_file ./data/g1/dance1_subject2_rev.csv` 49 | 50 | ![Demo](img/dance.gif) 51 | 52 | 53 | You can use the following arguments to tune the magnitude of the motion (globally and group-wise): 54 | * `--scale`: global motion scaler. Default `0.5` 55 | * `--scale_arm`: arms, compound onto `--scale`. Default `1.0`. 56 | * `--scale_leg`: legs, compound onto `--scale`. Default `0.5`. 57 | * `--scale_waist`: waist, compund onto `--scale`. Default `0.5`. 58 | 59 | Play music 60 | * Upload the music file (e.g. `./music/robot_merge2.wav`) to G1 under `/home/unitree`. 61 | * Connect the G1 with a speaker (e.g. via the type-c port at its neck). 62 | * Set the default audio output device (sink) to the speaker. You can use `pactl list short sinks` to list the devices, and use `pactl set-default-sink speaker-id` to select the device. 63 | * (Optionally) Run `alsamixer` in the G1 terminal and adjust the volume. After Alsamixer is launched, press F6 to select the output device and use key board to increase/decrease the volume. 64 | 65 | `python run_retarget.py --mocap_file ./data/g1/dance1_subject2_rev.csv --music_file robot_merge2.wav --scale_arm 1.25` 66 | 67 | 68 | 69 | __Demo__ 70 | 71 | ``` 72 | # Run the above demo 73 | ./demo.sh dance 74 | 75 | # Kill the music if needed 76 | ./demo.sh kill 77 | ``` -------------------------------------------------------------------------------- /demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | case "$1" in 4 | dance) 5 | python run_retarget.py --mocap_file ./data/g1/dance1_subject2_rev.csv --music_file robot_merge2.wav --scale_arm 1.25 6 | ;; 7 | 8 | kill) 9 | sshpass -p 123 ssh unitree@192.168.123.164 pkill -f "aplay.*" 10 | ;; 11 | 12 | *) 13 | echo "Usage: $0 {dance|kill}" 14 | exit 1 15 | esac 16 | -------------------------------------------------------------------------------- /img/dance.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LambdaLabsML/unitree-retarget/b565cde5b660e4ebf05d39687877ccf212ae5749/img/dance.gif -------------------------------------------------------------------------------- /img/manual_ip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LambdaLabsML/unitree-retarget/b565cde5b660e4ebf05d39687877ccf212ae5749/img/manual_ip.png -------------------------------------------------------------------------------- /img/sanity.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LambdaLabsML/unitree-retarget/b565cde5b660e4ebf05d39687877ccf212ae5749/img/sanity.gif -------------------------------------------------------------------------------- /music/robot_merge2.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LambdaLabsML/unitree-retarget/b565cde5b660e4ebf05d39687877ccf212ae5749/music/robot_merge2.wav -------------------------------------------------------------------------------- /music/robot_merge3.wav: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LambdaLabsML/unitree-retarget/b565cde5b660e4ebf05d39687877ccf212ae5749/music/robot_merge3.wav -------------------------------------------------------------------------------- /run_check.py: -------------------------------------------------------------------------------- 1 | # This is the unitree python sdk's original example 2 | # Added here as a sanity check 3 | # The original file can be found here: 4 | # https://github.com/unitreerobotics/unitree_sdk2_python/blob/master/example/g1/low_level/g1_low_level_example.py 5 | 6 | import time 7 | import sys 8 | 9 | from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize 10 | from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize 11 | from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ 12 | from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ 13 | from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ 14 | from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ 15 | from unitree_sdk2py.utils.crc import CRC 16 | from unitree_sdk2py.utils.thread import RecurrentThread 17 | from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient 18 | 19 | import numpy as np 20 | 21 | G1_NUM_MOTOR = 29 22 | 23 | Kp = [ 24 | 60, 60, 60, 100, 40, 40, # legs 25 | 60, 60, 60, 100, 40, 40, # legs 26 | 60, 40, 40, # waist 27 | 40, 40, 40, 40, 40, 40, 40, # arms 28 | 40, 40, 40, 40, 40, 40, 40 # arms 29 | ] 30 | 31 | Kd = [ 32 | 1, 1, 1, 2, 1, 1, # legs 33 | 1, 1, 1, 2, 1, 1, # legs 34 | 1, 1, 1, # waist 35 | 1, 1, 1, 1, 1, 1, 1, # arms 36 | 1, 1, 1, 1, 1, 1, 1 # arms 37 | ] 38 | 39 | class G1JointIndex: 40 | LeftHipPitch = 0 41 | LeftHipRoll = 1 42 | LeftHipYaw = 2 43 | LeftKnee = 3 44 | LeftAnklePitch = 4 45 | LeftAnkleB = 4 46 | LeftAnkleRoll = 5 47 | LeftAnkleA = 5 48 | RightHipPitch = 6 49 | RightHipRoll = 7 50 | RightHipYaw = 8 51 | RightKnee = 9 52 | RightAnklePitch = 10 53 | RightAnkleB = 10 54 | RightAnkleRoll = 11 55 | RightAnkleA = 11 56 | WaistYaw = 12 57 | WaistRoll = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked 58 | WaistA = 13 # NOTE: INVALID for g1 23dof/29dof with waist locked 59 | WaistPitch = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked 60 | WaistB = 14 # NOTE: INVALID for g1 23dof/29dof with waist locked 61 | LeftShoulderPitch = 15 62 | LeftShoulderRoll = 16 63 | LeftShoulderYaw = 17 64 | LeftElbow = 18 65 | LeftWristRoll = 19 66 | LeftWristPitch = 20 # NOTE: INVALID for g1 23dof 67 | LeftWristYaw = 21 # NOTE: INVALID for g1 23dof 68 | RightShoulderPitch = 22 69 | RightShoulderRoll = 23 70 | RightShoulderYaw = 24 71 | RightElbow = 25 72 | RightWristRoll = 26 73 | RightWristPitch = 27 # NOTE: INVALID for g1 23dof 74 | RightWristYaw = 28 # NOTE: INVALID for g1 23dof 75 | 76 | 77 | class Mode: 78 | PR = 0 # Series Control for Pitch/Roll Joints 79 | AB = 1 # Parallel Control for A/B Joints 80 | 81 | class Custom: 82 | def __init__(self): 83 | self.time_ = 0.0 84 | self.control_dt_ = 0.002 # [2ms] 85 | self.duration_ = 3.0 # [3 s] 86 | self.counter_ = 0 87 | self.mode_pr_ = Mode.PR 88 | self.mode_machine_ = 0 89 | self.low_cmd = unitree_hg_msg_dds__LowCmd_() 90 | self.low_state = None 91 | self.update_mode_machine_ = False 92 | self.crc = CRC() 93 | 94 | def Init(self): 95 | self.msc = MotionSwitcherClient() 96 | self.msc.SetTimeout(5.0) 97 | self.msc.Init() 98 | 99 | status, result = self.msc.CheckMode() 100 | while result['name']: 101 | self.msc.ReleaseMode() 102 | status, result = self.msc.CheckMode() 103 | time.sleep(1) 104 | 105 | # create publisher # 106 | self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_) 107 | self.lowcmd_publisher_.Init() 108 | 109 | # create subscriber # 110 | self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_) 111 | self.lowstate_subscriber.Init(self.LowStateHandler, 10) 112 | 113 | def Start(self): 114 | self.lowCmdWriteThreadPtr = RecurrentThread( 115 | interval=self.control_dt_, target=self.LowCmdWrite, name="control" 116 | ) 117 | while self.update_mode_machine_ == False: 118 | time.sleep(1) 119 | 120 | if self.update_mode_machine_ == True: 121 | self.lowCmdWriteThreadPtr.Start() 122 | 123 | def LowStateHandler(self, msg: LowState_): 124 | self.low_state = msg 125 | 126 | if self.update_mode_machine_ == False: 127 | self.mode_machine_ = self.low_state.mode_machine 128 | self.update_mode_machine_ = True 129 | 130 | self.counter_ +=1 131 | if (self.counter_ % 500 == 0) : 132 | self.counter_ = 0 133 | print(self.low_state.imu_state.rpy) 134 | 135 | def LowCmdWrite(self): 136 | self.time_ += self.control_dt_ 137 | 138 | if self.time_ < self.duration_ : 139 | # [Stage 1]: set robot to zero posture 140 | for i in range(G1_NUM_MOTOR): 141 | ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0) 142 | self.low_cmd.mode_pr = Mode.PR 143 | self.low_cmd.mode_machine = self.mode_machine_ 144 | self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable 145 | self.low_cmd.motor_cmd[i].tau = 0. 146 | self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q 147 | self.low_cmd.motor_cmd[i].dq = 0. 148 | self.low_cmd.motor_cmd[i].kp = Kp[i] 149 | self.low_cmd.motor_cmd[i].kd = Kd[i] 150 | 151 | elif self.time_ < self.duration_ * 2 : 152 | # [Stage 2]: swing ankle using PR mode 153 | max_P = np.pi * 30.0 / 180.0 154 | max_R = np.pi * 10.0 / 180.0 155 | t = self.time_ - self.duration_ 156 | L_P_des = max_P * np.sin(2.0 * np.pi * t) 157 | L_R_des = max_R * np.sin(2.0 * np.pi * t) 158 | R_P_des = max_P * np.sin(2.0 * np.pi * t) 159 | R_R_des = -max_R * np.sin(2.0 * np.pi * t) 160 | 161 | self.low_cmd.mode_pr = Mode.PR 162 | self.low_cmd.mode_machine = self.mode_machine_ 163 | self.low_cmd.motor_cmd[G1JointIndex.LeftAnklePitch].q = L_P_des 164 | self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleRoll].q = L_R_des 165 | self.low_cmd.motor_cmd[G1JointIndex.RightAnklePitch].q = R_P_des 166 | self.low_cmd.motor_cmd[G1JointIndex.RightAnkleRoll].q = R_R_des 167 | 168 | else : 169 | # [Stage 3]: swing ankle using AB mode 170 | max_A = np.pi * 30.0 / 180.0 171 | max_B = np.pi * 10.0 / 180.0 172 | t = self.time_ - self.duration_ * 2 173 | L_A_des = max_A * np.sin(2.0 * np.pi * t) 174 | L_B_des = max_B * np.sin(2.0 * np.pi * t + np.pi) 175 | R_A_des = -max_A * np.sin(2.0 * np.pi * t) 176 | R_B_des = -max_B * np.sin(2.0 * np.pi * t + np.pi) 177 | 178 | self.low_cmd.mode_pr = Mode.AB 179 | self.low_cmd.mode_machine = self.mode_machine_ 180 | self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleA].q = L_A_des 181 | self.low_cmd.motor_cmd[G1JointIndex.LeftAnkleB].q = L_B_des 182 | self.low_cmd.motor_cmd[G1JointIndex.RightAnkleA].q = R_A_des 183 | self.low_cmd.motor_cmd[G1JointIndex.RightAnkleB].q = R_B_des 184 | 185 | max_WristYaw = np.pi * 30.0 / 180.0 186 | L_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t) 187 | R_WristYaw_des = max_WristYaw * np.sin(2.0 * np.pi * t) 188 | self.low_cmd.motor_cmd[G1JointIndex.LeftWristRoll].q = L_WristYaw_des 189 | self.low_cmd.motor_cmd[G1JointIndex.RightWristRoll].q = R_WristYaw_des 190 | 191 | 192 | self.low_cmd.crc = self.crc.Crc(self.low_cmd) 193 | self.lowcmd_publisher_.Write(self.low_cmd) 194 | 195 | if __name__ == '__main__': 196 | 197 | print("WARNING: Please ensure there are no obstacles around the robot while running this example.") 198 | input("Press Enter to continue...") 199 | 200 | if len(sys.argv)>1: 201 | ChannelFactoryInitialize(0, sys.argv[1]) 202 | else: 203 | ChannelFactoryInitialize(0) 204 | 205 | custom = Custom() 206 | custom.Init() 207 | custom.Start() 208 | 209 | while True: 210 | time.sleep(1) -------------------------------------------------------------------------------- /run_retarget.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import argparse 4 | import subprocess 5 | import atexit 6 | import signal 7 | 8 | from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize 9 | from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ 10 | from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ 11 | from unitree_sdk2py.utils.crc import CRC 12 | from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient 13 | 14 | # Number of motors on the G1 robot 15 | G1_NUM_MOTOR = 29 16 | 17 | # PD gains for the 29 joints (using the same gains as in g1_low_level_example.py) 18 | # Kp = [ 19 | # 60, 60, 60, 100, 40, 40, # left leg 20 | # 60, 60, 60, 100, 40, 40, # right leg 21 | # 60, 40, 40, # waist 22 | # 40, 40, 40, 40, 40, 40, 40, # left arm 23 | # 40, 40, 40, 40, 40, 40, 40 # right arm 24 | # ] 25 | Kp = [ 26 | 30, 30, 30, 50, 20, 20, # left leg 27 | 30, 30, 30, 50, 20, 20, # right leg 28 | 30, 20, 20, # waist 29 | 10, 10, 10, 10, 10, 10, 10, # left arm 30 | 10, 10, 10, 10, 10, 10, 10 # right arm 31 | ] 32 | Kd = [ 33 | 1, 1, 1, 2, 1, 1, # left leg 34 | 1, 1, 1, 2, 1, 1, # right leg 35 | 1, 1, 1, # waist 36 | 1, 1, 1, 1, 1, 1, 1, # left arm 37 | 1, 1, 1, 1, 1, 1, 1 # right arm 38 | ] 39 | 40 | class MocapRetarget: 41 | def __init__(self, mocap_file, num_frames): 42 | # Load mocap CSV data. Each row has 36 values: 43 | # - First 7: base pose (position + quaternion) [ignored for low-level joint control] 44 | # - Last 29: joint angles (in radians) 45 | self.data = np.loadtxt(mocap_file, delimiter=',') 46 | # self.num_frames = self.data.shape[0] 47 | if num_frames == 0: 48 | self.num_frames = self.data.shape[0] 49 | else: 50 | self.num_frames = num_frames 51 | self.fps = 30.0 52 | self.scale = 1.0 53 | self.scale_leg = 0.0 # New scaling factor for legs (motors 0-11) 54 | self.scale_waist = 0.0 # New scaling factor for waist (motors 12-14) 55 | self.scale_arm = 0.0 # New scaling factor for arms (motors 15-28) 56 | self.mocap_skip = 7 57 | self.control_dt = 1.0 / self.fps 58 | self.low_cmd = unitree_hg_msg_dds__LowCmd_() # default constructor as in original example 59 | self.low_state = None 60 | self.crc = CRC() 61 | # Warmup time in seconds (default 2 sec) 62 | self.warmup_time = 2.0 63 | 64 | def low_state_handler(self, msg: unitree_hg_msg_dds__LowState_): 65 | self.low_state = msg 66 | 67 | def init_communication(self): 68 | # Initialize the channel factory 69 | ChannelFactoryInitialize(0) 70 | 71 | # Create and initialize publisher and subscriber 72 | self.lowcmd_pub = ChannelPublisher("rt/lowcmd", LowCmd_) 73 | self.lowcmd_pub.Init() 74 | self.lowstate_sub = ChannelSubscriber("rt/lowstate", LowState_) 75 | self.lowstate_sub.Init(self.low_state_handler, 10) 76 | 77 | # Initialize the Motion Switcher Client to ensure no other control mode is active 78 | msc = MotionSwitcherClient() 79 | msc.SetTimeout(5.0) 80 | msc.Init() 81 | status, result = msc.CheckMode() 82 | if result is None: 83 | print("Warning: No response from CheckMode. Proceeding under assumption that no other control mode is active.") 84 | else: 85 | while result.get('name'): 86 | msc.ReleaseMode() 87 | status, result = msc.CheckMode() 88 | time.sleep(1) 89 | 90 | # Wait until the first low_state is received 91 | print("Waiting for robot state...") 92 | while self.low_state is None: 93 | time.sleep(0.001) 94 | self.mode_machine = self.low_state.mode_machine 95 | print("Robot state acquired; mode_machine =", self.mode_machine) 96 | 97 | def send_motor_cmd(self): 98 | """Helper to send the current low_cmd message.""" 99 | self.low_cmd.mode_pr = 0 # Mode.PR for pitch/roll control 100 | self.low_cmd.mode_machine = self.mode_machine 101 | self.low_cmd.crc = self.crc.Crc(self.low_cmd) 102 | self.lowcmd_pub.Write(self.low_cmd) 103 | 104 | def warmup_transition(self): 105 | """ 106 | Smoothly interpolate from the current robot joint positions (neutral) 107 | to the first mocap frame over warmup_time seconds. 108 | """ 109 | # Determine number of warmup frames 110 | num_warmup_frames = int(self.warmup_time * self.fps) 111 | # Get current joint positions from low_state; fallback to zeros if not available. 112 | current_pose = [0.0] * G1_NUM_MOTOR 113 | if self.low_state is not None and hasattr(self.low_state, 'motor_state'): 114 | for i in range(G1_NUM_MOTOR): 115 | current_pose[i] = self.low_state.motor_state[i].q 116 | 117 | # Get the target pose from the first mocap frame (scale applied) 118 | target_pose = self.data[0, self.mocap_skip:] * self.scale 119 | 120 | print(f"Warmup transition over {num_warmup_frames} frames...") 121 | next_time = time.time() 122 | for frame in range(num_warmup_frames): 123 | next_time += self.control_dt 124 | ratio = (frame + 1) / num_warmup_frames 125 | interp_pose = [(1 - ratio) * current_pose[i] + ratio * target_pose[i] for i in range(G1_NUM_MOTOR)] 126 | for i in range(G1_NUM_MOTOR): 127 | self.low_cmd.motor_cmd[i].mode = 1 128 | self.low_cmd.motor_cmd[i].q = float(interp_pose[i]) 129 | self.low_cmd.motor_cmd[i].dq = 0.0 130 | self.low_cmd.motor_cmd[i].tau = 0.0 131 | self.low_cmd.motor_cmd[i].kp = Kp[i] 132 | self.low_cmd.motor_cmd[i].kd = Kd[i] 133 | self.send_motor_cmd() 134 | sleep_duration = next_time - time.time() 135 | if sleep_duration > 0: 136 | time.sleep(sleep_duration) 137 | print("Warmup transition complete.") 138 | 139 | def run(self): 140 | # Begin motion playback 141 | next_time = time.time() 142 | print(f"Starting motion playback at {self.fps} FPS...") 143 | for frame_idx in range(self.num_frames): 144 | next_time += self.control_dt 145 | # Extract joint angles from mocap data and apply scaling. 146 | joint_angles = self.data[frame_idx, self.mocap_skip:] * self.scale 147 | 148 | # Apply leg scaling (motors 0-11) 149 | joint_angles[0:12] *= (self.scale_leg) 150 | # Apply waist scaling (motors 12-14) 151 | joint_angles[12:15] *= (self.scale_waist) 152 | # Apply arm scaling (motors 15:29) 153 | joint_angles[15:29] *= (self.scale_arm) 154 | 155 | for i in range(G1_NUM_MOTOR): 156 | self.low_cmd.motor_cmd[i].q = float(joint_angles[i]) 157 | self.send_motor_cmd() 158 | 159 | sleep_duration = next_time - time.time() 160 | if sleep_duration > 0: 161 | time.sleep(sleep_duration) 162 | print("Motion playback finished.") 163 | 164 | if __name__ == "__main__": 165 | # Set up argument parser 166 | parser = argparse.ArgumentParser(description='Run mocap retargeting on G1 robot with warmup transition') 167 | parser.add_argument('--mocap_file', type=str, required=True, 168 | help='Path to the mocap CSV file. Currenlty support unitreerobotics/LAFAN1_Retargeting_Dataset') 169 | parser.add_argument('--fps', type=float, default=30.0, 170 | help='Playback frames per second (default: 30.0)') 171 | parser.add_argument('--scale', type=float, default=0.5, 172 | help='Scaling factor for joint angles (default: 0.5)') 173 | parser.add_argument('--scale_leg', type=float, default=0.5, 174 | help='Additional scaling factor for leg joints (motors 0-11) (default: 0.0)') 175 | parser.add_argument('--scale_waist', type=float, default=0.5, 176 | help='Additional scaling factor for waist joints (motors 12-14) (default: 0.0)') 177 | parser.add_argument('--scale_arm', type=float, default=1.0, 178 | help='Additional scaling factor for arm joints (motors 15-28) (default: 0.0)') 179 | parser.add_argument('--mocap_skip', type=int, default=7, 180 | help='Number of initial columns to skip in CSV (default: 7 for data from unitreerobotics/LAFAN1_Retargeting_Dataset)') 181 | parser.add_argument('--warmup_time', type=float, default=2.0, 182 | help='Warmup time (in seconds) to interpolate to initial pose (default: 2.0)') 183 | parser.add_argument('--offset', type=int, default=0, 184 | help='Starting joint index offset (default: 0 for full body). Change it to 15 for upper body (left & right arms) only.') 185 | parser.add_argument('--num_frames', type=int, default=0, 186 | help='num of frames for replay') 187 | parser.add_argument('--music_file', type=str, default="", 188 | help='Music file to play during motion (default: empty string)') 189 | args = parser.parse_args() 190 | 191 | # Initialize and run the retargeting 192 | retarget = MocapRetarget(args.mocap_file, args.num_frames) 193 | retarget.fps = args.fps 194 | retarget.scale = args.scale 195 | retarget.scale_leg = args.scale_leg 196 | retarget.scale_waist = args.scale_waist 197 | retarget.scale_arm = args.scale_arm 198 | retarget.mocap_skip = args.mocap_skip 199 | retarget.warmup_time = args.warmup_time 200 | 201 | retarget.init_communication() 202 | # Execute the warmup transition to smoothly move to the initial mocap pose. 203 | retarget.warmup_transition() 204 | 205 | # play music 206 | if args.music_file: 207 | # First, kill any existing audio processes 208 | cleanup_cmd = ['ssh', 'unitree@192.168.123.164', 'pkill -f "aplay.*"'] 209 | try: 210 | subprocess.run(['sshpass', '-p', '123'] + cleanup_cmd) 211 | except Exception as e: 212 | print(f"Warning: Could not cleanup existing audio processes: {e}") 213 | 214 | # Start the new audio loop 215 | ssh_command = ['ssh', 'unitree@192.168.123.164', 'while true; do aplay /home/unitree/'+ args.music_file + '; done'] 216 | audio_process = None 217 | try: 218 | audio_process = subprocess.Popen(['sshpass', '-p', '123'] + ssh_command) 219 | 220 | # Register cleanup function to be called on program exit 221 | def cleanup(): 222 | if audio_process: 223 | # Kill the SSH process 224 | audio_process.terminate() 225 | audio_process.wait() 226 | # Kill any remaining aplay processes on the remote machine 227 | try: 228 | subprocess.run(['sshpass', '-p', '123'] + cleanup_cmd) 229 | except: 230 | pass 231 | 232 | atexit.register(cleanup) 233 | signal.signal(signal.SIGINT, lambda sig, frame: exit(0)) # Handle Ctrl+C 234 | 235 | except Exception as e: 236 | print(f"Warning: Could not play audio file: {e}") 237 | 238 | # Run the main playback loop. 239 | retarget.run() 240 | --------------------------------------------------------------------------------