Merge pull request #6 from fan-ziqi/devel

feat: [Destructive Update] NEW VERSION
fan-ziqi · May 24, 2024 · 4370cbf · 4370cbf
2 parents 5d43cae + 174a268
commit 4370cbf
Show file tree

Hide file tree

Showing 69 changed files with 576 additions and 11,066 deletions.
diff --git a/README.md b/README.md
@@ -75,21 +75,10 @@ Open a new terminal, launch the gazebo simulation environment
 
 ```bash
 source devel/setup.bash
-roslaunch rl_sar start_a1.launch
+roslaunch rl_sar gazebo_a1.launch
 ```
 
-Open a new terminal, run the control program
-
-```bash
-source devel/setup.bash
-rosrun rl_sar rl_sim
-```
-
-Open a new terminal, run the keyboard control program
-
-```bash
-rosrun teleop_twist_keyboard teleop_twist_keyboard.py
-```
+Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
 
 ### Physical Robots
 
@@ -109,61 +98,7 @@ rosrun rl_sar rl_real_a1
 
 Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right.
 
-#### Cyberdog1
-
-1. Connect to the robot (only need to do this once)
-
-    Connect the local PC to the Cyberdog's USB download Type-C interface (located in the middle) and wait for the "L4T-README" pop-up to appear.
-
-    ```bash
-    ping 192.168.55.100     # IP assigned to the local PC
-    ssh [email protected]     # Log in to the NX application board, password 123
-    athena_version -v # Verify the current version is >=1.0.0.94
-    ```
-
-2. Enter motor control mode (only need to do this once)
-
-    Modify the configuration switch to activate user control mode and run the user's own controller:
-
-    ```bash
-    ssh [email protected] # Log in to the motion control board
-    cd /robot
-    ./initialize.sh # Copy factory code to the readable and writable development area (/mnt/UDISK/robot-software), switch to developer mode, only need to be executed once
-    vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt # Switch mode: 1 (0: default mode, 1 user code control motor mode), reboot the robot to take effect
-    ```
-
-3. Use Ethernet cable to connect the computer and the motion control board
-
-    Due to the risk of damaging the interface and higher communication latency when using a Type-C connection, it is recommended to use an Ethernet cable for connection. Disconnect the cables between the main control and motion control board of the robot, and connect the computer and the motion control board directly with an Ethernet cable, then set the wired connection IPv4 of the computer to manual ` 192.168.55.100`. It is recommended to remove the head and lead the cable out of the head opening. Be careful not to damage the cables during disassembly and assembly.
-
-    Initialize the robot's connection (this step needs to be done every time the robot is reconnected)
-
-    ```bash
-    cd src/rl_sar/scripts
-    bash init_cyberdog.sh
-    ```
-
-    Start the control program
-
-    ```bash
-    source devel/setup.bash
-    rosrun rl_sar rl_real_cyberdog
-    ```
-
-    Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
-
-4. Use a Type-C cable to connect the computer and the robot
-
-    If it is inconvenient to disassemble the robot, a Type-C cable can be temporarily used for debugging. The procedure after connecting the Type-C cable is the same as above.
-
-5. After using Ctrl+C to end the program, the robot's motion control program will automatically reset. If the program goes out of control, the motion control program can also be manually restarted.
-
-    Note: After restarting the motion control program, there is a startup time of approximately 5-10 seconds. During this time, running programs may report `Motor control mode has not been activated successfully`. Wait until there are no errors before running the control program again.
-
-    ```bash
-    cd src/rl_sar/scripts
-    bash kill_cyberdog.sh
-    ```
+OR Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
 
 ## Add Your Robot
 

diff --git a/README_CN.md b/README_CN.md
@@ -75,21 +75,10 @@ catkin build
 
 ```bash
 source devel/setup.bash
-roslaunch rl_sar start_a1.launch
+roslaunch rl_sar gazebo_a1.launch
 ```
 
-新建终端，启动控制程序
-
-```bash
-source devel/setup.bash
-rosrun rl_sar rl_sim
-```
-
-新建终端，键盘控制程序
-
-```bash
-rosrun teleop_twist_keyboard teleop_twist_keyboard.py
-```
+按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
 
 ### 实物
 
@@ -109,61 +98,7 @@ rosrun rl_sar rl_real_a1
 
 按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。
 
-#### Cyberdog1
-
-1. 连接机器人(只需执行一次此步骤)
-
-    将本地PC连接至铁蛋的USB download type-c 接口(位于中间)，等待出现”L4T-README” 弹窗
-
-    ```bash
-    ping 192.168.55.100     #本地PC被分配的ip
-    ssh [email protected]     #登录nx应用板 ,密码123
-    athena_version -v #核对当前版本>=1.0.0.94
-    ```
-
-2. 进入电机控制模式(只需执行一次此步骤)
-
-    修改配置开关，激活用户控制模式，运行用户自己的控制器：
-
-    ```bash
-    ssh [email protected] #登录运动控制板
-    cd /robot
-    ./initialize.sh #拷贝出厂代码到可读写的开发区（/mnt/UDISK/robot-software），切换到开发者模式，仅需执行一次
-    vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt #切换mode:1(0:默认模式，1用户代码控制电机模式),重启机器人生效
-    ```
-
-3. 使用网线连接电脑和运动控制板
-
-    由于使用Type-C连接时调试碰撞易损坏接口，而且通信延迟较高，故推荐使用网线进行连接。需要将机器人拆开，断开断开主控和运动控制板的网线，将电脑和运动控制板使用网线直接连接，并设置电脑的有线连接IPv4为手动`192.168.55.100`。推荐拆掉头部并将网线从头部的开口引出。拆装时候注意不要损坏排线。
-
-    初始化机器人的连接(每次重新连接机器人都要执行此步骤)
-
-    ```bash
-    cd src/rl_sar/scripts
-    bash init_cyberdog.sh
-    ```
-
-    启动控制程序
-
-    ```bash
-    source devel/setup.bash
-    rosrun rl_sar rl_real_cyberdog
-    ```
-
-    按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
-
-4. 使用Type-C线连接电脑与机器人
-
-    若不方便拆卸机器人，可以暂时使用Type-C线调试。接入Type-C线后运行方法同上。
-
-5. 程序在使用Ctrl+C结束后会自动重置机器人的运控程序，如程序失控也可手动重启运控程序。
-
-   注：运控程序重启后大概有5-10秒的启动时间，在这段时间内运行程序会报`Motor control mode has not been activated successfully`，需等待不报错再运行控制程序。
-
-    ```bash
-    cd src/rl_sar/scripts
-    bash kill_cyberdog.sh
-    ```
+OR 按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
 
 ## 添加你的机器人
 

diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt
@@ -43,16 +43,14 @@ include_directories(library/unitree_legged_sdk_3.2/include)
 link_directories(library/unitree_legged_sdk_3.2/lib)
 set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm)
 
-add_subdirectory(library/cyberdog_motor_sdk)
-include_directories(library/cyberdog_motor_sdk/include)
-
 include_directories(
     include
     ${catkin_INCLUDE_DIRS}
     ${unitree_legged_sdk_INCLUDE_DIRS}
     library/matplotlibcpp
     library/observation_buffer
     library/rl_sdk
+    library/loop
 )
 
 add_library(rl_sdk library/rl_sdk/rl_sdk.cpp)
@@ -75,14 +73,8 @@ target_link_libraries(rl_sim
   rl_sdk observation_buffer yaml-cpp
 )
 
-# add_executable(rl_real_a1 src/rl_real_a1.cpp )
-# target_link_libraries(rl_real_a1
-#   ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
-#   rl_sdk observation_buffer yaml-cpp
-# )
-
-# add_executable(rl_real_cyberdog src/rl_real_cyberdog.cpp )
-# target_link_libraries(rl_real_cyberdog
-#   ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
-#   rl_sdk observation_buffer cyberdog_motor_sdk yaml-cpp
-# )
+add_executable(rl_real_a1 src/rl_real_a1.cpp )
+target_link_libraries(rl_real_a1
+  ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
+  rl_sdk observation_buffer yaml-cpp
+)
diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml
@@ -3,16 +3,22 @@ a1:
   num_observations: 45
   clip_obs: 100.0
   clip_actions: 100.0
-  # damping: 0.5
-  # stiffness: 20.0
-  p_gains: [20, 20, 20, # FL
-            20, 20, 20, # FR
-            20, 20, 20, # RL
-            20, 20, 20] # RR
-  d_gains: [0.5, 0.5, 0.5, # FL
-            0.5, 0.5, 0.5, # FR
-            0.5, 0.5, 0.5, # RL
-            0.5, 0.5, 0.5] # RR
+  rl_kp: [20, 20, 20, # FL
+          20, 20, 20, # FR
+          20, 20, 20, # RL
+          20, 20, 20] # RR
+  rl_kd: [0.5, 0.5, 0.5, # FL
+          0.5, 0.5, 0.5, # FR
+          0.5, 0.5, 0.5, # RL
+          0.5, 0.5, 0.5] # RR
+  fixed_kp: [80, 80, 80, # FL
+             80, 80, 80, # FR
+             80, 80, 80, # RL
+             80, 80, 80] # RR
+  fixed_kd: [3, 3, 3, # FL
+             3, 3, 3, # FR
+             3, 3, 3, # RL
+             3, 3, 3] # RR
   action_scale: 0.25
   hip_scale_reduction: 0.5
   hip_scale_reduction_indices: [0, 3, 6, 9]
@@ -40,16 +46,22 @@ cyberdog1:
   num_observations: 45
   clip_obs: 100.0
   clip_actions: 100.0
-  # damping: 0.5
-  # stiffness: 20.0
-  p_gains: [20, 20, 20, # FL
-            20, 20, 20, # FR
-            20, 20, 20, # RL
-            20, 20, 20] # RR
-  d_gains: [0.5, 0.5, 0.5, # FL
-            0.5, 0.5, 0.5, # FR
-            0.5, 0.5, 0.5, # RL
-            0.5, 0.5, 0.5] # RR
+  rl_kp: [20, 20, 20, # FL
+          20, 20, 20, # FR
+          20, 20, 20, # RL
+          20, 20, 20] # RR
+  rl_kd: [0.5, 0.5, 0.5, # FL
+          0.5, 0.5, 0.5, # FR
+          0.5, 0.5, 0.5, # RL
+          0.5, 0.5, 0.5] # RR
+  fixed_kp: [80, 80, 80, # FL
+             80, 80, 80, # FR
+             80, 80, 80, # RL
+             80, 80, 80] # RR
+  fixed_kd: [3, 3, 3, # FL
+             3, 3, 3, # FR
+             3, 3, 3, # RL
+             3, 3, 3] # RR
   action_scale: 0.25
   hip_scale_reduction: 0.5
   hip_scale_reduction_indices: [0, 3, 6, 9]
@@ -76,16 +88,22 @@ lite3_wheel:
   num_observations: 45
   clip_obs: 100.0
   clip_actions: 100.0
-  # damping: 0.5
-  # stiffness: 20.0
-  p_gains: [20, 20, 20, 0, # FL
-            20, 20, 20, 0, # FR
-            20, 20, 20, 0, # RL
-            20, 20, 20, 0] # RR
-  d_gains: [0.5, 0.5, 0.5, 0.5, # FL
-            0.5, 0.5, 0.5, 0.5, # FR
-            0.5, 0.5, 0.5, 0.5, # RL
-            0.5, 0.5, 0.5, 0.5] # RR
+  rl_kp: [20, 20, 20, 0, # FL
+          20, 20, 20, 0, # FR
+          20, 20, 20, 0, # RL
+          20, 20, 20, 0] # RR
+  rl_kd: [0.5, 0.5, 0.5, 0.5, # FL
+          0.5, 0.5, 0.5, 0.5, # FR
+          0.5, 0.5, 0.5, 0.5, # RL
+          0.5, 0.5, 0.5, 0.5] # RR
+  fixed_kp: [80, 80, 80, # FL
+             80, 80, 80, # FR
+             80, 80, 80, # RL
+             80, 80, 80] # RR
+  fixed_kd: [3, 3, 3, # FL
+             3, 3, 3, # FR
+             3, 3, 3, # RL
+             3, 3, 3] # RR
   action_scale: 0.25
   hip_scale_reduction: 1.0
   hip_scale_reduction_indices: [0, 3, 6, 9]
@@ -113,12 +131,14 @@ gr1t1:
   num_observations: 39
   clip_obs: 100.0
   clip_actions: 100.0
-  # damping: 0.5
-  # stiffness: 20.0
-  p_gains: [57.0, 43.0, 114.0, 114.0, 15.3, 
-            57.0, 43.0, 114.0, 114.0, 15.3]
-  d_gains: [5.7, 4.3, 11.4, 11.4, 1.5, 
-            5.7, 4.3, 11.4, 11.4, 1.5]
+  rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3, 
+          57.0, 43.0, 114.0, 114.0, 15.3]
+  rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 
+          5.7, 4.3, 11.4, 11.4, 1.5]
+  fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3, 
+             57.0, 43.0, 114.0, 114.0, 15.3]
+  fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5, 
+             5.7, 4.3, 11.4, 11.4, 1.5]
   action_scale: 1.0
   hip_scale_reduction: 1.0
   hip_scale_reduction_indices: []