Deploy on physical robots
This commit is contained in:
craipy
parent
fb7514ad38
commit
3e70454dae
89
README.md
89
README.md
|
|
@ -2,14 +2,14 @@
|
|||
|
||||
This is a simple example of using Unitree Robots for reinforcement learning, including Unitree Go2, H1, H1_2, G1
|
||||
|
||||
### Installation
|
||||
## 1. Installation
|
||||
|
||||
1. Create a new python virtual env with python 3.6, 3.7 or 3.8 (3.8 recommended)
|
||||
2. Install pytorch 1.10 with cuda-11.3:
|
||||
1. Create a new python virtual env with python 3.8
|
||||
|
||||
```
|
||||
pip3 install torch==1.10.0+cu113 torchvision==0.11.1+cu113 torchaudio==0.10.0+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html
|
||||
2. Install pytorch 2.3.1 with cuda-12.1:
|
||||
|
||||
```bash
|
||||
pip install torch==2.3.1 torchvision==0.18.1 torchaudio==2.3.1 --index-url https://download.pytorch.org/whl/cu121
|
||||
```
|
||||
3. Install Isaac Gym
|
||||
|
||||
|
|
@ -27,7 +27,12 @@ This is a simple example of using Unitree Robots for reinforcement learning, inc
|
|||
- Navigate to the folder `unitree_rl_gym`
|
||||
- `pip install -e .`
|
||||
|
||||
### Usage
|
||||
6. Install unitree_sdk2py (Optional for depoly on real robot)
|
||||
|
||||
- Clone [https://github.com/unitreerobotics/unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python)
|
||||
- `cd unitree_sdk2_python & pip install -e .`
|
||||
|
||||
## 2. Train in Isaac Gym
|
||||
|
||||
1. Train:
|
||||
`python legged_gym/scripts/train.py --task=go2`
|
||||
|
|
@ -51,65 +56,41 @@ This is a simple example of using Unitree Robots for reinforcement learning, inc
|
|||
* By default, the loaded policy is the last model of the last run of the experiment folder.
|
||||
* Other runs/model iteration can be selected by setting `load_run` and `checkpoint` in the train config.
|
||||
|
||||
### Robots Demo
|
||||
|
||||
1. Go2
|
||||
|
||||
https://github.com/user-attachments/assets/98395d82-d3f6-4548-b6ee-8edfce70ac3e
|
||||
|
||||
2. H1
|
||||
|
||||
https://github.com/user-attachments/assets/7762b4f9-1072-4794-8ef6-7dd253a7ad4c
|
||||
|
||||
3. H1-2
|
||||
|
||||
https://github.com/user-attachments/assets/695323a7-a2d9-445b-bda8-f1b697159c39
|
||||
|
||||
4. G1
|
||||
|
||||
https://github.com/user-attachments/assets/6063c03e-1143-4c75-8fda-793c8615cb08
|
||||
### 2.1 Play Demo
|
||||
|
||||
|
||||
### mujoco(sim2sim)
|
||||
| Go2 | G1 | H1 | H1_2 |
|
||||
|--- | --- | --- | --- |
|
||||
| <video src="https://github.com/user-attachments/assets/98395d82-d3f6-4548-b6ee-8edfce70ac3e" controls="controls" width="400px"></video> | <video src="https://github.com/user-attachments/assets/2d2c2ea9-7816-4754-a3f0-d6d23912c569" controls="controls" width="400px"></video> | <video src="https://github.com/user-attachments/assets/622c8e3f-e82d-413a-b0ff-b0aca0fa22e5" controls="controls" width="400px"></video> | <video src="https://github.com/user-attachments/assets/1ec25467-4101-49c1-8c35-5c109e49e81e" controls="controls" width="400px"></video> |
|
||||
|
||||
1. H1
|
||||
## 3. Sim in Mujoco
|
||||
|
||||
Execute the following command in the project path:
|
||||
### 3.1 Mujoco Usage
|
||||
|
||||
To execute sim2sim in mujoco, execute the following command:
|
||||
|
||||
```bash
|
||||
python deploy/deploy_mujoco/deploy_mujoco.py {config_name}
|
||||
```
|
||||
|
||||
`config_name`: The file name of the configuration file. The configuration file will be found under `deploy/deploy_mujoco/configs/`, for example `g1.yaml`, `h1.yaml`, `h1_2.yaml`.
|
||||
|
||||
**example**:
|
||||
|
||||
```bash
|
||||
python deploy/deploy_mujoco/deploy_mujoco.py g1.yaml
|
||||
|
||||
```
|
||||
|
||||
Then you can get the following effect:
|
||||
### 3.2 Mujoco Demo
|
||||
|
||||
https://github.com/user-attachments/assets/10a84f8d-c02f-41cb-b2fd-76a97951b2c3
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
| <video src="https://github.com/user-attachments/assets/9455b595-791e-4715-b280-0e70b9d45c53" controls="controls" width="400px"></video> | <video src="https://github.com/user-attachments/assets/2ff75b99-0186-4195-9ec8-69901e7b6700" controls="controls" width="400px"></video> | <video src="https://github.com/user-attachments/assets/8e3476ff-3d5c-45d4-a227-7565bf885d93" controls="controls" width="400px"></video> |
|
||||
|
||||
2. H1_2
|
||||
## 4. Depoly on Physical Robot
|
||||
|
||||
Execute the following command in the project path:
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
| [<img src="https://oss-global-cdn.unitree.com/static/c5667475f51844628911cf032509d80a_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/621806fb837c4f869e5c59efd1d93105.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/42d2332dc3004097896f33d0db027039_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/9c61509fc4f74d21bb707a5fe3ae11aa.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/c49a03fa297a4d178ec3a5b01b9c0bbf_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/e60a0fcd829e417f92a88e78463a695d.mp4) |
|
||||
|
||||
```bash
|
||||
|
||||
python deploy/deploy_mujoco/deploy_mujoco.py h1_2.yaml
|
||||
|
||||
```
|
||||
|
||||
Then you can get the following effect:
|
||||
|
||||
https://github.com/user-attachments/assets/fdd4f53d-3235-4978-a77f-1c71b32fb301
|
||||
|
||||
3. G1
|
||||
|
||||
Execute the following command in the project path:
|
||||
|
||||
```bash
|
||||
|
||||
python deploy/deploy_mujoco/deploy_mujoco.py g1.yaml
|
||||
|
||||
```
|
||||
|
||||
Then you can get the following effect:
|
||||
|
||||
https://github.com/user-attachments/assets/99b892c3-7886-49f4-a7f1-0420b51443dd
|
||||
reference to [Deploy on Physical Robot(English)](deploy/deploy_real/README.md) | [实物部署（简体中文）](deploy/deploy_real/README.zh.md)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,79 @@
|
|||
# Deploy on Physical Robot
|
||||
|
||||
This code can deploy the trained network on physical robots. Currently supported robots include Unitree G1, H1, H1_2.
|
||||
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
|  |  |  |
|
||||
|
||||
|
||||
## Startup Usage
|
||||
|
||||
```bash
|
||||
python deploy_real.py {net_interface} {config_name}
|
||||
```
|
||||
|
||||
- `net_interface`: is the name of the network interface connected to the robot, such as `enp3s0`
|
||||
- `config_name`: is the file name of the configuration file. The configuration file will be found under `deploy/deploy_real/configs/`, such as `g1.yaml`, `h1.yaml`, `h1_2.yaml`.
|
||||
|
||||
## Startup Process
|
||||
|
||||
### 1. Start the robot
|
||||
|
||||
Start the robot in the hoisting state and wait for the robot to enter `zero torque mode`
|
||||
|
||||
### 2. Enter the debugging mode
|
||||
|
||||
Make sure the robot is in `zero torque mode`, press the `L2+R2` key combination of the remote control; the robot will enter the `debugging mode`, and the robot joints are in the damping state in the `debugging mode`.
|
||||
|
||||
|
||||
### 3. Connect the robot
|
||||
|
||||
Use an Ethernet cable to connect your computer to the network port on the robot. Modify the network configuration as follows
|
||||
|
||||
<img src="https://doc-cdn.unitree.com/static/2023/9/6/0f51cb9b12f94f0cb75070d05118c00a_980x816.jpg" width="400px">
|
||||
|
||||
Then use the `ifconfig` command to view the name of the network interface connected to the robot. Record the network interface name, which will be used as a parameter of the startup command later
|
||||
|
||||
<img src="https://oss-global-cdn.unitree.com/static/b84485f386994ef08b0ccfa928ab3830_825x484.png" width="400px">
|
||||
|
||||
### 4. Start the program
|
||||
|
||||
Assume that the network card currently connected to the physical robot is named `enp3s0`. Take the G1 robot as an example, execute the following command to start
|
||||
|
||||
```bash
|
||||
python deploy_real.py enp3s0 g1.yaml
|
||||
```
|
||||
|
||||
#### 4.1 Zero torque state
|
||||
|
||||
After starting, the robot joints will be in the zero torque state. You can shake the robot joints by hand to feel and confirm.
|
||||
|
||||
#### 4.2 Default position state
|
||||
|
||||
In the zero torque state, press the `start` button on the remote control, and the robot will move to the default joint position state.
|
||||
|
||||
After the robot moves to the default joint position, you can slowly lower the hoisting mechanism to let the robot's feet touch the ground.
|
||||
|
||||
#### 4.3 Motion control mode
|
||||
|
||||
After the preparation is completed, press the `A` button on the remote control, and the robot will step on the spot. After the robot is stable, you can gradually lower the hoisting rope to give the robot a certain amount of space to move.
|
||||
|
||||
At this time, you can use the joystick on the remote control to control the movement of the robot.
|
||||
The front and back of the left joystick controls the movement speed of the robot in the x direction
|
||||
The left and right of the left joystick controls the movement speed of the robot in the y direction
|
||||
The left and right of the right joystick controls the movement speed of the robot's yaw angle
|
||||
|
||||
#### 4.4 Exit control
|
||||
|
||||
In motion control mode, press the `select` button on the remote control, the robot will enter the damping mode and fall down, and the program will exit. Or use `ctrl+c` in the terminal to close the program.
|
||||
|
||||
> Note:
|
||||
>
|
||||
> Since this example deployment is not a stable control program and is only used for demonstration purposes, please try not to disturb the robot during the control process. If any unexpected situation occurs during the control process, please exit the control in time to avoid danger.
|
||||
|
||||
## Video tutorial
|
||||
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
| [<img src="https://oss-global-cdn.unitree.com/static/c5667475f51844628911cf032509d80a_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/621806fb837c4f869e5c59efd1d93105.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/42d2332dc3004097896f33d0db027039_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/9c61509fc4f74d21bb707a5fe3ae11aa.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/c49a03fa297a4d178ec3a5b01b9c0bbf_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/e60a0fcd829e417f92a88e78463a695d.mp4) |
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
# 实物部署
|
||||
|
||||
本代码可以在实物部署训练的网络。目前支持的机器人包括 Unitree G1, H1, H1_2。
|
||||
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
|  |  |  |
|
||||
|
||||
## 启动用法
|
||||
|
||||
```bash
|
||||
python deploy_real.py {net_interface} {config_name}
|
||||
```
|
||||
|
||||
- `net_interface`: 为连接机器人的网卡的名字，例如`enp3s0`
|
||||
- `config_name`: 配置文件的文件名。配置文件会在 `deploy/deploy_real/configs/` 下查找， 例如`g1.yaml`, `h1.yaml`, `h1_2.yaml`。
|
||||
|
||||
## 启动过程
|
||||
|
||||
### 1. 启动机器人
|
||||
|
||||
将机器人在吊装状态下启动，并等待机器人进入 `零力矩模式`
|
||||
|
||||
### 2. 进入调试模式
|
||||
|
||||
确保机器人处于 `零力矩模式` 的情况下，按下遥控器的 `L2+R2`组合键；此时机器人会进入`调试模式`, `调试模式`下机器人关节处于阻尼状态。
|
||||
|
||||
### 3. 连接机器人
|
||||
|
||||
使用网线连接自己的电脑和机器人上的网口。修改网络配置如下
|
||||
|
||||
<img src="https://doc-cdn.unitree.com/static/2023/9/6/0f51cb9b12f94f0cb75070d05118c00a_980x816.jpg" width="400px">
|
||||
|
||||
然后使用 `ifconfig` 命令查看与机器人连接的网卡的名称。网卡名称记录下来，后面会作为启动命令的参数
|
||||
|
||||
<img src="https://oss-global-cdn.unitree.com/static/b84485f386994ef08b0ccfa928ab3830_825x484.png" width="400px">
|
||||
|
||||
### 4. 启动程序
|
||||
|
||||
假设目前与实物机器人连接的网卡名为`enp3s0`.以G1机器人为例，执行下面的命令启动
|
||||
|
||||
```bash
|
||||
python deploy_real.py enp3s0 g1.yaml
|
||||
```
|
||||
|
||||
#### 4.1 零力矩状态
|
||||
|
||||
启动之后，机器人关节会处于零力矩状态，可以用手晃动机器人的关节感受并确认一下。
|
||||
|
||||
#### 4.2 默认位置状态
|
||||
|
||||
在零力矩状态时，按下遥控器上的`start`按键，机器人会运动到默认关节位置状态。
|
||||
|
||||
在机器人运动到默认关节位置之后，可以缓慢的下放吊装机构，让机器人的脚与地面接触。
|
||||
|
||||
#### 4.3 运动控制模式
|
||||
|
||||
准备工作完成，按下遥控器上`A`键，机器人此时会原地踏步，在机器人状态稳定之后，可以逐渐降低吊装绳，给机器人一定的活动空间。
|
||||
|
||||
此时使用遥控器上的摇杆就可以控制机器人的运动了。
|
||||
左摇杆的前后，控制机器人的x方向的运动速度
|
||||
左摇杆的左右，控制机器人的y方向的运动速度
|
||||
右摇杆的左右，控制机器人的偏航角yaw的运动速度
|
||||
|
||||
#### 4.4 退出控制
|
||||
|
||||
在运动控制模式下，按下遥控器上 `select` 按键，机器人会进入阻尼模式倒下，程序退出。或者在终端中 使用 `ctrl+c` 关闭程序。
|
||||
|
||||
> 注意：
|
||||
>
|
||||
> 由于本示例部署并非稳定的控制程序，仅用于示例作用，请控制过程尽量不要给机器人施加扰动，如果控制过程中出现任何意外情况，请及时退出控制，以免发生危险。
|
||||
|
||||
## 视频教程
|
||||
|
||||
| G1 | H1 | H1_2 |
|
||||
|--- | --- | --- |
|
||||
| [<img src="https://oss-global-cdn.unitree.com/static/8e7132fc3062493db93e4eb5005908f4_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/e6c659515ccd437294709e5abb6ae8d8.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/41803de7a8384fa799509ddedb505db4_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/56a0160154d1468f9ed9628e4a43254e.mp4) | [<img src="https://oss-global-cdn.unitree.com/static/76109eb66b7a4d46a347019b1053c4c7_1920x1080.png" width="400px">](https://oss-global-cdn.unitree.com/static/f34ab7b6a7b14e058bbaebfce712db3a.mp4) |
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
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
|
||||
|
|
@ -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]
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
from legged_gym import LEGGED_GYM_ROOT_DIR
|
||||
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"].replace("{LEGGED_GYM_ROOT_DIR}", LEGGED_GYM_ROOT_DIR)
|
||||
|
||||
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"]
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#
|
||||
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: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/g1/motion.pt"
|
||||
|
||||
leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
|
||||
kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40]
|
||||
kds: [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 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]
|
||||
|
||||
arm_waist_joint2motor_idx: [12, 13, 14,
|
||||
15, 16, 17, 18, 19, 20, 21,
|
||||
22, 23, 24, 25, 26, 27, 28]
|
||||
|
||||
arm_waist_kps: [300, 300, 300,
|
||||
100, 100, 50, 50, 20, 20, 20,
|
||||
100, 100, 50, 50, 20, 20, 20]
|
||||
|
||||
arm_waist_kds: [3, 3, 3,
|
||||
2, 2, 2, 2, 1, 1, 1,
|
||||
2, 2, 2, 2, 1, 1, 1]
|
||||
|
||||
arm_waist_target: [ 0, 0, 0,
|
||||
0, 0, 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: 12
|
||||
num_obs: 47
|
||||
|
||||
max_cmd: [0.8, 0.5, 1.57]
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
#
|
||||
control_dt: 0.02
|
||||
|
||||
msg_type: "go" # "hg" or "go"
|
||||
imu_type: "torso" # "torso" or "pelvis"
|
||||
|
||||
weak_motor: [10, 11, 12, 13, 14, 15, 16, 17, 18, 19]
|
||||
|
||||
lowcmd_topic: "rt/lowcmd"
|
||||
lowstate_topic: "rt/lowstate"
|
||||
|
||||
policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/h1/motion.pt"
|
||||
|
||||
leg_joint2motor_idx: [7, 3, 4, 5, 10, 8, 0, 1, 2, 11]
|
||||
kps: [150, 150, 150, 200, 40, 150, 150, 150, 200, 40]
|
||||
kds: [2, 2, 2, 4, 2, 2, 2, 2, 4, 2]
|
||||
default_angles: [0, 0.0, -0.1, 0.3, -0.2,
|
||||
0, 0.0, -0.1, 0.3, -0.2]
|
||||
|
||||
arm_waist_joint2motor_idx: [6,
|
||||
16, 17, 18, 19,
|
||||
12, 13, 14, 15]
|
||||
|
||||
arm_waist_kps: [300,
|
||||
100, 100, 50, 50,
|
||||
100, 100, 50, 50]
|
||||
|
||||
arm_waist_kds: [3,
|
||||
2, 2, 2, 2,
|
||||
2, 2, 2, 2]
|
||||
|
||||
arm_waist_target: [ 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: 10
|
||||
num_obs: 41
|
||||
|
||||
max_cmd: [0.8, 0.5, 1.57]
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
#
|
||||
control_dt: 0.02
|
||||
|
||||
msg_type: "hg" # "hg" or "go"
|
||||
imu_type: "torso" # "torso" or "pelvis"
|
||||
|
||||
lowcmd_topic: "rt/lowcmd"
|
||||
lowstate_topic: "rt/lowstate"
|
||||
|
||||
policy_path: "{LEGGED_GYM_ROOT_DIR}/deploy/pre_train/h1_2/motion.pt"
|
||||
|
||||
leg_joint2motor_idx: [0, 1, 2, 3, 4, 5,
|
||||
6, 7, 8, 9, 10, 11]
|
||||
kps: [200, 200, 200, 300, 40, 40,
|
||||
200, 200, 200, 300, 40, 40]
|
||||
kds: [2.5, 2.5, 2.5, 4, 2, 2,
|
||||
2.5, 2.5, 2.5, 4, 2, 2]
|
||||
default_angles: [0, -0.16, 0.0, 0.36, -0.2, 0.0,
|
||||
0, -0.16, 0.0, 0.36, -0.2, 0.0]
|
||||
|
||||
arm_waist_joint2motor_idx: [12,
|
||||
13, 14, 15, 16, 17, 18, 19,
|
||||
20, 21, 22, 23, 24, 25, 26]
|
||||
|
||||
arm_waist_kps: [300,
|
||||
120, 120, 120, 80, 80, 80, 80,
|
||||
120, 120, 120, 80, 80, 80, 80]
|
||||
|
||||
arm_waist_kds: [3,
|
||||
2, 2, 2, 1, 1, 1, 1,
|
||||
2, 2, 2, 1, 1, 1, 1]
|
||||
|
||||
arm_waist_target: [ 0,
|
||||
0, 0, 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: 12
|
||||
num_obs: 47
|
||||
|
||||
max_cmd: [0.8, 0.5, 1.57]
|
||||
|
|
@ -0,0 +1,265 @@
|
|||
from legged_gym import LEGGED_GYM_ROOT_DIR
|
||||
from typing import Union
|
||||
import numpy as np
|
||||
import time
|
||||
import torch
|
||||
|
||||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
|
||||
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_
|
||||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_, unitree_go_msg_dds__LowState_
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as LowCmdHG
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as LowCmdGo
|
||||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as LowStateHG
|
||||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ as LowStateGo
|
||||
from unitree_sdk2py.utils.crc import CRC
|
||||
|
||||
from common.command_helper import create_damping_cmd, create_zero_cmd, init_cmd_hg, init_cmd_go, MotorMode
|
||||
from common.rotation_helper import get_gravity_orientation, transform_imu_data
|
||||
from common.remote_controller import RemoteController, KeyMap
|
||||
from config import Config
|
||||
|
||||
|
||||
class Controller:
|
||||
def __init__(self, config: Config) -> None:
|
||||
self.config = config
|
||||
self.remote_controller = RemoteController()
|
||||
|
||||
# Initialize the policy network
|
||||
self.policy = torch.jit.load(config.policy_path)
|
||||
# Initializing process variables
|
||||
self.qj = np.zeros(config.num_actions, dtype=np.float32)
|
||||
self.dqj = np.zeros(config.num_actions, dtype=np.float32)
|
||||
self.action = np.zeros(config.num_actions, dtype=np.float32)
|
||||
self.target_dof_pos = config.default_angles.copy()
|
||||
self.obs = np.zeros(config.num_obs, dtype=np.float32)
|
||||
self.cmd = np.array([0.0, 0, 0])
|
||||
self.counter = 0
|
||||
|
||||
if config.msg_type == "hg":
|
||||
# g1 and h1_2 use the hg msg type
|
||||
self.low_cmd = unitree_hg_msg_dds__LowCmd_()
|
||||
self.low_state = unitree_hg_msg_dds__LowState_()
|
||||
self.mode_pr_ = MotorMode.PR
|
||||
self.mode_machine_ = 0
|
||||
|
||||
self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdHG)
|
||||
self.lowcmd_publisher_.Init()
|
||||
|
||||
self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateHG)
|
||||
self.lowstate_subscriber.Init(self.LowStateHgHandler, 10)
|
||||
|
||||
elif config.msg_type == "go":
|
||||
# h1 uses the go msg type
|
||||
self.low_cmd = unitree_go_msg_dds__LowCmd_()
|
||||
self.low_state = unitree_go_msg_dds__LowState_()
|
||||
|
||||
self.lowcmd_publisher_ = ChannelPublisher(config.lowcmd_topic, LowCmdGo)
|
||||
self.lowcmd_publisher_.Init()
|
||||
|
||||
self.lowstate_subscriber = ChannelSubscriber(config.lowstate_topic, LowStateGo)
|
||||
self.lowstate_subscriber.Init(self.LowStateGoHandler, 10)
|
||||
|
||||
else:
|
||||
raise ValueError("Invalid msg_type")
|
||||
|
||||
# wait for the subscriber to receive data
|
||||
self.wait_for_low_state()
|
||||
|
||||
# Initialize the command msg
|
||||
if config.msg_type == "hg":
|
||||
init_cmd_hg(self.low_cmd, self.mode_machine_, self.mode_pr_)
|
||||
elif config.msg_type == "go":
|
||||
init_cmd_go(self.low_cmd, weak_motor=self.config.weak_motor)
|
||||
|
||||
def LowStateHgHandler(self, msg: LowStateHG):
|
||||
self.low_state = msg
|
||||
self.mode_machine_ = self.low_state.mode_machine
|
||||
self.remote_controller.set(self.low_state.wireless_remote)
|
||||
|
||||
def LowStateGoHandler(self, msg: LowStateGo):
|
||||
self.low_state = msg
|
||||
self.remote_controller.set(self.low_state.wireless_remote)
|
||||
|
||||
def send_cmd(self, cmd: Union[LowCmdGo, LowCmdHG]):
|
||||
cmd.crc = CRC().Crc(cmd)
|
||||
self.lowcmd_publisher_.Write(cmd)
|
||||
|
||||
def wait_for_low_state(self):
|
||||
while self.low_state.tick == 0:
|
||||
time.sleep(self.config.control_dt)
|
||||
print("Successfully connected to the robot.")
|
||||
|
||||
def zero_torque_state(self):
|
||||
print("Enter zero torque state.")
|
||||
print("Waiting for the start signal...")
|
||||
while self.remote_controller.button[KeyMap.start] != 1:
|
||||
create_zero_cmd(self.low_cmd)
|
||||
self.send_cmd(self.low_cmd)
|
||||
time.sleep(self.config.control_dt)
|
||||
|
||||
def move_to_default_pos(self):
|
||||
print("Moving to default pos.")
|
||||
# move time 2s
|
||||
total_time = 2
|
||||
num_step = int(total_time / self.config.control_dt)
|
||||
|
||||
dof_idx = self.config.leg_joint2motor_idx + self.config.arm_waist_joint2motor_idx
|
||||
kps = self.config.kps + self.config.arm_waist_kps
|
||||
kds = self.config.kds + self.config.arm_waist_kds
|
||||
default_pos = np.concatenate((self.config.default_angles, self.config.arm_waist_target), axis=0)
|
||||
dof_size = len(dof_idx)
|
||||
|
||||
# record the current pos
|
||||
init_dof_pos = np.zeros(dof_size, dtype=np.float32)
|
||||
for i in range(dof_size):
|
||||
init_dof_pos[i] = self.low_state.motor_state[dof_idx[i]].q
|
||||
|
||||
# move to default pos
|
||||
for i in range(num_step):
|
||||
alpha = i / num_step
|
||||
for j in range(dof_size):
|
||||
motor_idx = dof_idx[j]
|
||||
target_pos = default_pos[j]
|
||||
self.low_cmd.motor_cmd[motor_idx].q = init_dof_pos[j] * (1 - alpha) + target_pos * alpha
|
||||
self.low_cmd.motor_cmd[motor_idx].qd = 0
|
||||
self.low_cmd.motor_cmd[motor_idx].kp = kps[j]
|
||||
self.low_cmd.motor_cmd[motor_idx].kd = kds[j]
|
||||
self.low_cmd.motor_cmd[motor_idx].tau = 0
|
||||
self.send_cmd(self.low_cmd)
|
||||
time.sleep(self.config.control_dt)
|
||||
|
||||
def default_pos_state(self):
|
||||
print("Enter default pos state.")
|
||||
print("Waiting for the Button A signal...")
|
||||
while self.remote_controller.button[KeyMap.A] != 1:
|
||||
for i in range(len(self.config.leg_joint2motor_idx)):
|
||||
motor_idx = self.config.leg_joint2motor_idx[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].q = self.config.default_angles[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].qd = 0
|
||||
self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].tau = 0
|
||||
for i in range(len(self.config.arm_waist_joint2motor_idx)):
|
||||
motor_idx = self.config.arm_waist_joint2motor_idx[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].qd = 0
|
||||
self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].tau = 0
|
||||
self.send_cmd(self.low_cmd)
|
||||
time.sleep(self.config.control_dt)
|
||||
|
||||
def run(self):
|
||||
self.counter += 1
|
||||
# Get the current joint position and velocity
|
||||
for i in range(len(self.config.leg_joint2motor_idx)):
|
||||
self.qj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].q
|
||||
self.dqj[i] = self.low_state.motor_state[self.config.leg_joint2motor_idx[i]].dq
|
||||
|
||||
# imu_state quaternion: w, x, y, z
|
||||
quat = self.low_state.imu_state.quaternion
|
||||
ang_vel = np.array([self.low_state.imu_state.gyroscope], dtype=np.float32)
|
||||
|
||||
if self.config.imu_type == "torso":
|
||||
# h1 and h1_2 imu is on the torso
|
||||
# imu data needs to be transformed to the pelvis frame
|
||||
waist_yaw = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].q
|
||||
waist_yaw_omega = self.low_state.motor_state[self.config.arm_waist_joint2motor_idx[0]].dq
|
||||
quat, ang_vel = transform_imu_data(waist_yaw=waist_yaw, waist_yaw_omega=waist_yaw_omega, imu_quat=quat, imu_omega=ang_vel)
|
||||
|
||||
# create observation
|
||||
gravity_orientation = get_gravity_orientation(quat)
|
||||
qj_obs = self.qj.copy()
|
||||
dqj_obs = self.dqj.copy()
|
||||
qj_obs = (qj_obs - self.config.default_angles) * self.config.dof_pos_scale
|
||||
dqj_obs = dqj_obs * self.config.dof_vel_scale
|
||||
ang_vel = ang_vel * self.config.ang_vel_scale
|
||||
period = 0.8
|
||||
count = self.counter * self.config.control_dt
|
||||
phase = count % period / period
|
||||
sin_phase = np.sin(2 * np.pi * phase)
|
||||
cos_phase = np.cos(2 * np.pi * phase)
|
||||
|
||||
self.cmd[0] = self.remote_controller.ly
|
||||
self.cmd[1] = self.remote_controller.lx * -1
|
||||
self.cmd[2] = self.remote_controller.rx * -1
|
||||
|
||||
num_actions = self.config.num_actions
|
||||
self.obs[:3] = ang_vel
|
||||
self.obs[3:6] = gravity_orientation
|
||||
self.obs[6:9] = self.cmd * self.config.cmd_scale * self.config.max_cmd
|
||||
self.obs[9 : 9 + num_actions] = qj_obs
|
||||
self.obs[9 + num_actions : 9 + num_actions * 2] = dqj_obs
|
||||
self.obs[9 + num_actions * 2 : 9 + num_actions * 3] = self.action
|
||||
self.obs[9 + num_actions * 3] = sin_phase
|
||||
self.obs[9 + num_actions * 3 + 1] = cos_phase
|
||||
|
||||
# Get the action from the policy network
|
||||
obs_tensor = torch.from_numpy(self.obs).unsqueeze(0)
|
||||
self.action = self.policy(obs_tensor).detach().numpy().squeeze()
|
||||
|
||||
# transform action to target_dof_pos
|
||||
target_dof_pos = self.config.default_angles + self.action * self.config.action_scale
|
||||
|
||||
# Build low cmd
|
||||
for i in range(len(self.config.leg_joint2motor_idx)):
|
||||
motor_idx = self.config.leg_joint2motor_idx[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].q = target_dof_pos[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].qd = 0
|
||||
self.low_cmd.motor_cmd[motor_idx].kp = self.config.kps[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].kd = self.config.kds[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].tau = 0
|
||||
|
||||
for i in range(len(self.config.arm_waist_joint2motor_idx)):
|
||||
motor_idx = self.config.arm_waist_joint2motor_idx[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].q = self.config.arm_waist_target[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].qd = 0
|
||||
self.low_cmd.motor_cmd[motor_idx].kp = self.config.arm_waist_kps[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].kd = self.config.arm_waist_kds[i]
|
||||
self.low_cmd.motor_cmd[motor_idx].tau = 0
|
||||
|
||||
# send the command
|
||||
self.send_cmd(self.low_cmd)
|
||||
|
||||
time.sleep(self.config.control_dt)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("net", type=str, help="network interface")
|
||||
parser.add_argument("config", type=str, help="config file name in the configs folder", default="g1.yaml")
|
||||
args = parser.parse_args()
|
||||
|
||||
# Load config
|
||||
config_path = f"{LEGGED_GYM_ROOT_DIR}/deploy/deploy_real/configs/{args.config}"
|
||||
config = Config(config_path)
|
||||
|
||||
# Initialize DDS communication
|
||||
ChannelFactoryInitialize(0, args.net)
|
||||
|
||||
controller = Controller(config)
|
||||
|
||||
# Enter the zero torque state, press the start key to continue executing
|
||||
controller.zero_torque_state()
|
||||
|
||||
# Move to the default position
|
||||
controller.move_to_default_pos()
|
||||
|
||||
# Enter the default position state, press the A key to continue executing
|
||||
controller.default_pos_state()
|
||||
|
||||
while True:
|
||||
try:
|
||||
controller.run()
|
||||
# Press the select key to exit
|
||||
if controller.remote_controller.button[KeyMap.select] == 1:
|
||||
break
|
||||
except KeyboardInterrupt:
|
||||
break
|
||||
# Enter the damping state
|
||||
create_damping_cmd(controller.low_cmd)
|
||||
controller.send_cmd(controller.low_cmd)
|
||||
print("Exit")
|
||||
Loading…
Reference in New Issue