update for hardware test (#4)

README.md

@@ -38,12 +38,12 @@ python play.py
 Go2 pretrained model is provided in [./runs](runs/gait-conditioned-agility/pretrain-go2), you can choose whether to use provide pretrained model by modifying the label line `label = "gait-conditioned-agility/pretrain-go2/train"` to your own trained model.
 
 ### Known Issues
-* `flip_visual_attachments` in [go1_config](go1_gym/envs/go1/go1_config.py) should be set to `True`, otherwise errors would occur when visualizing.
+* `flip_visual_attachments` in [go2_config](go2_gym/envs/go2/go2_config.py) should be set to `True`, otherwise errors would occur when visualizing.
-* To change configuration parameters of env or the robot, you should modify parameters in [go1_config](go1_gym/envs/go1/go1_config.py), not in [legged_robot_config](go1_gym/envs/base/legged_robot_config.py)
+* To change configuration parameters of env or the robot, you should modify parameters in [go2_config](go2_gym/envs/go2/go2_config.py), not in [legged_robot_config](go2_gym/envs/base/legged_robot_config.py)
 
 
 ---
-## Deploy
+## Deploy on PC
 Trained policy is only supported to be deployed through your PC or laptop now, because I am not familiar with Jetson Orin, and hope I can fix it and deploy on Jetson Orin.
 
 ### Requirements
@@ -132,11 +132,19 @@ Open a new terminate and run:
 cd go2_gym_deploy/scripts
 python deploy_policy.py
 ```
+According to the hints shown in terminal, Press button [R2] to start the controller. You can check RC mapping in the following subsection.
+
+
+### Joystick Mapping
+
+![Joystick Mapping](media/rc_map.png)
+
+
+To view the details of joystick mapping or even modify default mapping logic, please refer to the `get_command` function within the [cheetah_state_estimator.py](go2_gym_deploy/utils/cheetah_state_estimator.py) file. In this project, the default gait is set to trot.
 
-According to the hints shown in terminal, Press [R2] to start the controller. You can check RC mapping from [walk-these-ways](https://github.com/Improbable-AI/walk-these-ways) page.
 
 **Caution**:
-* Press [L2+B] if any unexpected situation occurs!!!
+* Press [L2+B] to switch to damping mode if any unexpected situation occurs!!!
 * This is research code; use at your own risk; we do not take responsibility for any damage.
 
 Test Video on Unitree Go2: 
@@ -144,7 +152,15 @@ Test Video on Unitree Go2:
 - Test by other contributors: https://www.bilibili.com/video/BV1Ut421H7Fr/?spm_id_from=333.1007.top_right_bar_window_history.content.click&vd_source=07873ebe2a113dac57775e264a210929
 
 
-**Please star this repository if it does help you! Many Thanks!**
+---
+## Deploy on Nvidia Jetson Orin
+
+To be continue:
+
+
+---
+
+🌟🌟🌟  **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.

go2_gym_deploy/unitree_sdk2_bin/lcm_position_go2.cpp

@@ -359,12 +359,28 @@ void Custom::LowCmdWrite(){
             low_cmd.motor_cmd()[i].kd() = 5;
             low_cmd.motor_cmd()[i].tau() = 0;
         }  
-        std::cout << "======= Switch to Damping Mode, and the thread is sleeping ========"<<std::endl;
+        std::cout << "======= Switched to Damping Mode, and the thread is sleeping ========"<<std::endl;
-        sleep(10);
+        sleep(0.5);
-        std::cout << "======= Damping is about to exit ========"<<std::endl;
+
-        sleep(10);
+        while (true)
-        std::cout << "======= Damping exit ========"<<std::endl;
+        {
+            sleep(0.5);
+            if ((int)key.components.B==1 && (int)key.components.L2==1) // [L2+B] is pressed again
+            {
                 exit(0);
+            }else if ((int)key.components.A==1 && (int)key.components.L2==1)
+            {
+                std::cout << "======= activate sport_mode service ========"<<std::endl;
+                rsc.ServiceSwitch("sport_mode", 1);
+                sleep(1);
+                exit(0);
+            }else
+            {   std::cout << "======= Press [L2+B] again to exit ========"<<std::endl;
+                std::cout << "======= If the robot is set to nominal pose manually, Press [L2+A] to activate sport_mode service ========"<<std::endl;
+                continue;
+            }
+        }
+        
     } 
     else{
         for (int i = 0; i < 12; i++)
@@ -431,35 +447,41 @@ int main(int argc, char **argv)
         exit(-1);
     }
 
-    std::cout << "Caution: Remember to shut down Unitree sport_mode Service." << std::endl
+    std::cout << "Communication level is set to LOW-level." << std::endl
-              << "Communication level is set to LOW-level." << std::endl
               << "WARNING: Make sure the robot is hung up." << std::endl
+              << "Caution: The scripts is about to shutdown Unitree sport_mode Service." << std::endl
               << "Press Enter to continue..." << std::endl;
     std::cin.ignore();
 
-    unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]); // 传入benji的网卡地址
+    unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]); // 传入本机的网卡地址（PC or Jetson Orin）
 
     Custom custom;
 
     custom.InitRobotStateClient();
-    while(custom.queryServiceStatus("sport_mode"))
+    if(custom.queryServiceStatus("sport_mode"))
     {
         std::cout<<"Trying to deactivate the service: " << "sport_mode" << std::endl;
         custom.activateService("sport_mode",0);
         sleep(1);
-    }
+    } else{
-    std::cout<<"sportd_mode is deactivated now" << std::endl
+        std::cout <<"sportd_mode is already deactivated now" << std::endl
+                  <<"next step is setting up communication" << std::endl
                   << "Press Enter to continue..." << std::endl;
         std::cin.ignore();
+    }
+
 
     custom.Init();
 
     std::cout<<"Communicatino is set up successfully" << std::endl;
     std::cout<<"LCM <<<------------>>> Unitree SDK2" << std::endl;
+    std::cout<<"------------------------------------" << std::endl;
+    std::cout<<"------------------------------------" << std::endl;
+    std::cout<<"Press L2+B if any unexpected error occurs" << std::endl;
 
     custom.Loop();
 
-    while (1)
+    while (true)
     {
         sleep(10);
     }

go2_gym_deploy/utils/cheetah_state_estimator.py

@@ -192,28 +192,28 @@ class StateEstimator:
             cmd_ori_pitch = -0.4 * self.right_stick[1]
 
         # gait buttons
-        if self.mode == 0:
+        if self.mode == 0: # Press Button 'A' -> 'Bound'
             self.cmd_phase = 0.5
             self.cmd_offset = 0.0
             self.cmd_bound = 0.0
             self.cmd_duration = 0.5
-        elif self.mode == 1:
+        elif self.mode == 1: # Press Button 'B' -> 'Trot'
             self.cmd_phase = 0.0
             self.cmd_offset = 0.0
             self.cmd_bound = 0.0
             self.cmd_duration = 0.5
-        elif self.mode == 2:
+        elif self.mode == 2: # Press Button 'X' -> 'Pace'
             self.cmd_phase = 0.0
             self.cmd_offset = 0.5
             self.cmd_bound = 0.0
             self.cmd_duration = 0.5
-        elif self.mode == 3:
+        elif self.mode == 3: # Press Button 'Y' -> 'Pronk'
             self.cmd_phase = 0.0
             self.cmd_offset = 0.0
             self.cmd_bound = 0.5
             self.cmd_duration = 0.5
-        else:
+        else: # Default Gait -> 'Trot'
-            self.cmd_phase = 0.5
+            self.cmd_phase = 0.0
             self.cmd_offset = 0.0
             self.cmd_bound = 0.0
             self.cmd_duration = 0.5