diff --git a/README.md b/README.md index 4e9fe17..9c12b19 100755 --- a/README.md +++ b/README.md @@ -4,15 +4,16 @@ This repository is forked from [walk-these-ways](https://github.com/Improbable-AI/walk-these-ways), which is a Go1 Sim-to-Real Locomotion Starter Kit. It seems that [walk-these-ways](https://github.com/Improbable-AI/walk-these-ways) can be untilized on Unitree [A1](https://github.com/fan-ziqi/dog_rl_deploy) with simple modifications, since those robots are base on [unitree-legged-sdk](https://github.com/unitreerobotics/unitree_legged_sdk). -However, the brand-new architecture [unitree-sdk2 ](https://github.com/unitreerobotics/unitree_sdk2)is not base on UDP anymore, so this project aims to train and deploy walk-these-ways on Unitree Go2 by modifying SDK interfaces. +However, the brand-new architecture [unitree-sdk2 ](https://github.com/unitreerobotics/unitree_sdk2)is not based on UDP anymore, so this project aims to train and deploy walk-these-ways on Unitree Go2 by modifying SDK interfaces. ## Requirements +* miniconda * pytorch 1.10 with cuda-11.3 * Isaac Gym * Nvidia GPU with at least 8GB of VRAM --- -## Train +## Train and Play Clone this repository and install: ``` bash @@ -114,9 +115,9 @@ If error occurs, please check [Unitree Support](https://support.unitree.com/home ### Test communication between LCM and unitree_sdk2 ```bash cd go2_gym_deploy/build -sudo ./lcm_position_go2 enx10086 +sudo ./lcm_position_go2 eth0 ``` -Aeplace `enx10086` with your own network interface address. According to the messages shown in terminal, press `Enter` for several times and the communication between LCM and unitree_sdk2 will set up. +Aeplace `eth0` with your own network interface address. According to the messages shown in terminal, press `Enter` for several times and the communication between LCM and unitree_sdk2 will set up. This command will automatically shut down Unitree sport_mode Service and set the robot to LOW-LEVEL. Please make sure This will Go2 is hung up or lie on the ground. @@ -126,6 +127,11 @@ cd go2_gym_deploy/build sudo ./lcm_receive ``` +If LCM and unitree_sdk2 are correctly connected with each other, messages will be shown in the terminal: + +![Alt text](media/lcm_receive.png) + + ### Load and run policy Open a new terminate and run: ```bash @@ -155,18 +161,88 @@ Test Video on Unitree Go2: --- ## Deploy on Nvidia Jetson Orin -To be continue: +The Unitree Go2 robot is equipped with an onboard Nvidia Jetson Orin Nano/NX, which operates on an ARM-based architecture. Default information of this onboard computer is shown below, and you can connnect to Jetson by SSH, VScode(remote development) or plugging a HDMI cable. + +``` +IP:192.168.123.18 +user name：unitree +password：123 +``` + +### Requirements for Jetson +- cuda +- pytorch +- miniconda (Omitted here; please install it by yourself) +- cudnn (Omitted here; please install it by yourself) + + +Two different ways are provided to set up correct environments in Jetson: through Internet or through Docker. + +### Through Internet +Connecting a Nvidia Jetson device to the internet can be done in two primary ways: + +1. **Wired Connection**: Directly plug an Ethernet cable with internet access into the Jetson's Ethernet port. This method provides a stable and fast internet connection, suitable for tasks that require high bandwidth or low latency. + +2. **Wireless Connection via USB Wi-Fi Adapter**: Purchase a USB Wi-Fi adapter compatible with the Jetson device. This method adds wireless connectivity, offering the flexibility to connect to the internet without the need for physical cables. However, it's important to ensure the USB Wi-Fi adapter is supported by the Jetson's operating system and drivers. + + + +#### Check Jetpack Version +Jetpack toolbox has been preinstalled on Jetson, you should check the jetpack vertsion firstly. +```bash +sudo -H pip install jetson-stats #Install jetson-stats toolkit +sudo jtop +``` +According to the detail information printed in the terminal window, the Jetpack version of my Unitree Go2 is `Jetpack 5.1.1 [L4T 35.3.1]` + +![](media/sudo_jtop.png) + +You can also check libraries that have been preinstalled: +```bash +sudo jetson_release +``` +#### Install cuda for jetson +Check if there is a preinstalled version of cuda. +```bash +nvcc -V # check preinstalled cuda version +``` + +If the preinstalled version if too high, you should uninstall it because, for instance, there is no Pytorch version that is compatible with cuda-12.2. + +```bash +sudo apt-get remove cuda +sudo apt autoremove +sudo apt-get remove cuda* +sudo dpkg -l |grep cuda # check if any residual cuda file exists +sudo dpkg -P Residual filename +``` + +Personally, I recommend to install cuda-11.8. Click the link, [CUDA Toolkit 11.8 Downloads](https://developer.nvidia.com/cuda-11-8-0-download-archive?target_os=Linux&target_arch=aarch64-jetson&Compilation=Native&Distribution=Ubuntu&target_version=20.04&target_type=deb_local) , to check installation commands. + +#### Install Pytorch for Jetson + +Please [download](https://forums.developer.nvidia.com/t/pytorch-for-jetson/72048) pre-built PyTorch pip wheel installers for Jetson Nano, which is different from the way we install Pytorch on PC. Note that correct pytorch version should be chosen to make it compatible with specific version of cuda and Jetpack. + +#### Run codes without cable +As long as the environment and requirements on the Jetson are properly configured, you can follow the same deployment guidelines as you would on a PC. This liberates the robot! Now, you can test the code cable-free, offering more freedom to the robot's movements and applications. + +### Through Docker + +To be continue ... + +--- + +🌟🌟🌟 **Please star this repository if it does help you! Many Thanks!** 🌟🌟🌟 --- - -🌟🌟🌟 **Please star this repository if it does help you! Many Thanks!** - ## Acknowledgements * Many thanks to [Leolar](https://github.com/NihaoyaLeolar), who provide Nvidia 3060ti and supporting. * Many thanks to [Jony](https://github.com/jonyzhang2023) and Peter for their support and encourage me to learn basic kownledge about RL. * Many thanks to [Simonforyou](https://github.com/Simonforyou), who provide Go2 pretrained model. +--- ## TO DO - [x] Do not inherit config and env from go1_gym, build customized config and env files for Go2 -- [ ] Deploy on Jeston Orin Nano +- [x] Deploy on Jeston Orin Nano +- [ ] Deploy through Docker diff --git a/go2_gym/envs/go2/go2_config.py b/go2_gym/envs/go2/go2_config.py index cb61105..fe81cbb 100755 --- a/go2_gym/envs/go2/go2_config.py +++ b/go2_gym/envs/go2/go2_config.py @@ -28,8 +28,8 @@ def config_go2(Cnfg: Union[Cfg, Meta]): _ = Cnfg.control _.control_type = 'P' # P - _.stiffness = {'joint': 20.} # [N*m/rad] - _.damping = {'joint': 0.5} # [N*m*s/rad] + _.stiffness = {'joint': 25.} # [N*m/rad] 关节PD参数有待调整 + _.damping = {'joint': 0.6} # [N*m*s/rad] 关节PD参数有待调整 # action scale: target angle = actionScale * action + defaultAngle _.action_scale = 0.25 _.hip_scale_reduction = 0.5 @@ -37,7 +37,6 @@ def config_go2(Cnfg: Union[Cfg, Meta]): _.decimation = 4 _ = Cnfg.asset - # _.file = '{MINI_GYM_ROOT_DIR}/resources/robots/go1/urdf/go1.urdf' _.file = '{MINI_GYM_ROOT_DIR}/resources/robots/go2/urdf/go2.urdf' _.foot_name = "foot" _.penalize_contacts_on = ["thigh", "calf"] diff --git a/go2_gym_deploy/envs/lcm_agent.py b/go2_gym_deploy/envs/lcm_agent.py index 2105299..ea97b1f 100755 --- a/go2_gym_deploy/envs/lcm_agent.py +++ b/go2_gym_deploy/envs/lcm_agent.py @@ -231,7 +231,7 @@ class LCMAgent(): self.actions = torch.clip(actions[0:1, :], -clip_actions, clip_actions) self.publish_action(self.actions, hard_reset=hard_reset) time.sleep(max(self.dt - (time.time() - self.time), 0)) - if self.timestep % 100 == 0: print(f'frq: {1 / (time.time() - self.time)} Hz'); + if self.timestep % 100 == 0: print(f'frq: {1 / (time.time() - self.time)} Hz') self.time = time.time() obs = self.get_obs() diff --git a/go2_gym_deploy/scripts/deploy_policy.py b/go2_gym_deploy/scripts/deploy_policy.py index 5f62432..27d13c4 100644 --- a/go2_gym_deploy/scripts/deploy_policy.py +++ b/go2_gym_deploy/scripts/deploy_policy.py @@ -10,6 +10,7 @@ from go2_gym_deploy.utils.command_profile import * import pathlib +# lcm多播通信的标准格式 lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") def load_and_run_policy(label, experiment_name, max_vel=1.0, max_yaw_vel=1.0): diff --git a/go2_gym_deploy/unitree_sdk2_bin/lcm_position_go2.cpp b/go2_gym_deploy/unitree_sdk2_bin/lcm_position_go2.cpp index 33eaa00..879da28 100644 --- a/go2_gym_deploy/unitree_sdk2_bin/lcm_position_go2.cpp +++ b/go2_gym_deploy/unitree_sdk2_bin/lcm_position_go2.cpp @@ -25,11 +25,15 @@ #define TOPIC_LOWSTATE "rt/lowstate" #define TOPIC_JOYSTICK "rt/wirelesscontroller" +// 为保证项目代码的稳定性和易理解，没有采用unitree_sdk2中采用的using namespace语句 + constexpr double PosStopF = (2.146E+9f); constexpr double VelStopF = (16000.0f); + +// 无需更改：Unitree 提供的电机校验函数 uint32_t crc32_core(uint32_t* ptr, uint32_t len) -{ // 无需更改：Unitree 提供的电机校验函数 +{ unsigned int xbit = 0; unsigned int data = 0; unsigned int CRC32 = 0xFFFFFFFF; @@ -61,7 +65,7 @@ uint32_t crc32_core(uint32_t* ptr, uint32_t len) } -// 遥控器键值联合体 +// 遥控器键值联合体，摘自unitree_sdk2，无需更改 typedef union { struct @@ -138,7 +142,7 @@ public: void Custom::InitRobotStateClient() { - rsc.SetTimeout(10.0f); + rsc.SetTimeout(5.0f); rsc.Init(); } @@ -189,6 +193,7 @@ void Custom::JoystickHandler(const void *message) // ------------------------------------------------------------------------------- // 线程 1 ： lcm send 线程 +// 此线程作用：实时通过unitree_sdk2读取low_state信号和joystick信号，并发送给lcm中间件 void Custom::lcm_send(){ // leg_control_lcm_data for (int i = 0; i < 12; i++) @@ -224,7 +229,6 @@ void Custom::lcm_send(){ rc_command.left_lower_left_switch = key.components.L2; rc_command.left_upper_switch = key.components.L1; - // 这里的mode是用来控制啥的？ if(key.components.A > 0){ mode = 0; } else if(key.components.B > 0){ @@ -253,22 +257,29 @@ void Custom::lcm_send(){ // std::cout << "loop: messsages are sending ......" << std::endl; } + +// ------------------------------------------------------------------------------- +// 线程 2 ： lcm receive 线程 +// 此线程作用：实时通过lcm中间件读取pytorch神经网络输出的期望关节控制信号（q, qd, kp, kd, tau_ff） +// 查看 go2_gym_deploy/envs/lcm_agent.py 文件，可以知道： +// 神经网络只输出期望的q，而kp，kd是可以自定义设置的, qd 和 tau_ff 被设置为0 void Custom::lcm_receive_Handler(const lcm::ReceiveBuffer *rbuf, const std::string & chan, const pd_tau_targets_lcmt* msg){ (void) rbuf; (void) chan; joint_command_simple = *msg; //接收神经网络输出的关节信号 } -// ------------------------------------------------------------------------------- -// 线程 2 ： lcm receive 线程 +// 此处参考lcm推荐的标准格式，循环处理，接受lcm消息 void Custom::lcm_receive(){ - // 循环处理，接受lcm消息 while(true){ lc.handle(); } } +// ------------------------------------------------------------------------------- +// 线程 3 ： unitree_sdk2 command write 线程 +// 此线程作用：初始化low_cmd，经过合理的状态机后，电机将执行神经网络的输出 void Custom::InitLowCmd() { //LowCmd 类型中的 head 成员 表示帧头， @@ -289,7 +300,6 @@ void Custom::InitLowCmd() 如果用户在调试过程中发现无法控制 Go2 机器人的关节电机， 请检查变量的值是否为0x01。*/ low_cmd.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode - low_cmd.motor_cmd()[i].q() = (PosStopF); low_cmd.motor_cmd()[i].dq() = (VelStopF); low_cmd.motor_cmd()[i].kp() = (0); @@ -298,16 +308,15 @@ void Custom::InitLowCmd() } } - void Custom::SetNominalPose(){ // 运行此cpp文件后，不仅是初始化通信 - // 同样会趴下时的初始化关节角度 + // 同样会在趴下时的初始化关节角度 // 将各个电机都设置为位置模式 for(int i = 0; i < 12; i++){ joint_command_simple.qd_des[i] = 0; joint_command_simple.tau_ff[i] = 0; - joint_command_simple.kp[i] = 60; // 关节PD参数有待调整 60 - joint_command_simple.kd[i] = 5; // 关节PD参数有待调整 5 + joint_command_simple.kp[i] = 20; + joint_command_simple.kd[i] = 0.5; } // 趴下时的关节角度 @@ -325,11 +334,8 @@ void Custom::SetNominalPose(){ joint_command_simple.q_des[11] = -2.721; std::cout<<"SET NOMINAL POSE"< 0.5 || low_state.imu_state().rpy()[1] > 0.5 || ((int)key.components.B==1 && (int)key.components.L2==1)) - if ( std::abs(low_state.imu_state().rpy()[0]) > 0.5 || std::abs(low_state.imu_state().rpy()[1]) > 0.5 || ((int)key.components.B==1 && (int)key.components.L2==1)) + if ( std::abs(low_state.imu_state().rpy()[0]) > 0.8 || std::abs(low_state.imu_state().rpy()[1]) > 0.8 || ((int)key.components.B==1 && (int)key.components.L2==1)) { - for (int i = 0; i < 12; i++) - { + for (int i = 0; i < 12; i++){ // 进入damping模式 low_cmd.motor_cmd()[i].q() = 0; low_cmd.motor_cmd()[i].dq() = 0; @@ -360,35 +367,46 @@ void Custom::LowCmdWrite(){ low_cmd.motor_cmd()[i].tau() = 0; } std::cout << "======= Switched to Damping Mode, and the thread is sleeping ========"<>> Unitree SDK2" << std::endl; + std::cout<<"------------------------------------" << std::endl; + std::cout<<"------------------------------------" << std::endl; + std::cout<<"Press L2+B if any unexpected error occurs" << std::endl; + break; + + }else{ + std::cout << "======= Press [L2+B] again to exit ========"<(TOPIC_JOYSTICK)); joystick_suber->InitChannel(std::bind(&Custom::JoystickHandler, this, std::placeholders::_1), 1); - } @@ -466,7 +488,10 @@ int main(int argc, char **argv) { std::cout<<"Trying to deactivate the service: " << "sport_mode" << std::endl; custom.activateService("sport_mode",0); - sleep(1); + sleep(0.5); + if(!custom.queryServiceStatus("sport_mode")){ + std::cout<<"Trying to deactivate the service: " << "sport_mode" << std::endl; + } } else{ std::cout <<"sportd_mode is already deactivated now" << std::endl <<"next step is setting up communication" << std::endl @@ -491,4 +516,4 @@ int main(int argc, char **argv) } return 0; -} +} \ No newline at end of file diff --git a/media/lcm_receive.png b/media/lcm_receive.png new file mode 100644 index 0000000..beb3296 Binary files /dev/null and b/media/lcm_receive.png differ diff --git a/media/sudo_jtop.png b/media/sudo_jtop.png new file mode 100644 index 0000000..9be5d20 Binary files /dev/null and b/media/sudo_jtop.png differ