mirror of https://github.com/fan-ziqi/rl_sar.git
v2.3 (#51)
## Changes 1. Fixed `observations_history` order to ensure consistency in data processing. 2. Removed row-column dependencies in `config.yaml` and explicitly specified joint order. 3. Simplified launch files: Now, different models for single robot can be launched specified using `cfg:=`. Example: ```bash roslaunch rl_sar gazebo_go2.launch cfg:=himloco ``` 4. Moved `command_mapping` and `state_mapping` from *.hpp to `config.yaml` for better configuration management. 5. Added simulation and deployment SDK for `GoldenRetriever-l4w4` robot. 6. Added support for pretrained models of `go2`, `go2w`, `b2`, `b2w`, `a1`. 7. Reorganized the project structure: - Moved SDK to `thirdparty/`. - Moved core libraries to `core/`. 8. Fixed duplicate `#ifndef` issues in hpp files.
This commit is contained in:
robotsfan
GitHub
parent
f13aac008a
commit
87be546a0c
|
|
@ -1,28 +0,0 @@
|
|||
name: Send submodule updates to parent repo
|
||||
|
||||
on:
|
||||
push:
|
||||
branches:
|
||||
- main
|
||||
|
||||
jobs:
|
||||
update:
|
||||
runs-on: ubuntu-latest
|
||||
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
with:
|
||||
repository: fan-ziqi/BlindDog
|
||||
token: ${{ secrets.PRIVATE_TOKEN_GITHUB }}
|
||||
|
||||
- name: Pull & update submodules recursively
|
||||
run: |
|
||||
git submodule update --init --recursive
|
||||
git submodule update --recursive --remote
|
||||
- name: Commit
|
||||
run: |
|
||||
git config user.email "actions@github.com"
|
||||
git config user.name "GitHub Actions - update submodules"
|
||||
git add --all
|
||||
git commit -m "Update submodules" || echo "No changes to commit"
|
||||
git push
|
||||
48
README.md
48
README.md
|
|
@ -1,5 +1,10 @@
|
|||
# rl_sar
|
||||
|
||||
[](https://ubuntu.com/)
|
||||
[](https://wiki.ros.org/noetic)
|
||||
[](https://wiki.ros.org/foxy)
|
||||
[](https://opensource.org/license/apache-2-0)
|
||||
|
||||
[中文文档](README_CN.md)
|
||||
|
||||
**Version Select: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy/Humble](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
|
||||
|
|
@ -7,14 +12,17 @@
|
|||
This repository provides a framework for simulation verification and physical deployment of robot reinforcement learning algorithms, suitable for quadruped robots, wheeled robots, and humanoid robots. "sar" stands for "simulation and real"
|
||||
|
||||
feature:
|
||||
- Support legged_gym based on IsaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish.
|
||||
- The code has two versions: **ROS-Noetic** and **ROS2-Foxy/Humble**
|
||||
- The code supports both cpp and python, you can find python version in `src/rl_sar/scripts`
|
||||
- Built-in pre-trained models for multiple robot simulations, including `Unitree-A1`, `Unitree-Go2`, `Unitree-Go2W`, `Unitree-B2`, `Unitree-B2W`, `FFTAI-GR1T1`, `FFTAI-GR1T2`, `GoldenRetriever-L4W0`, `GoldenRetriever-L4W4`;
|
||||
- The training framework supports **IsaacGym** and **IsaacSim**, distinguished by `framework`;
|
||||
- The code has **ROS-Noetic** and **ROS2-Foxy/Humble** versions;
|
||||
- The code has **cpp** and **python** versions, with the python version located in `src/rl_sar/scripts`;
|
||||
|
||||
> [!NOTE]
|
||||
> If you want to train policy using IsaacLab(IsaacSim), please use [robot_lab](https://github.com/fan-ziqi/robot_lab) project.
|
||||
>
|
||||
> [Click to discuss on Discord](https://discord.gg/vmVjkhVugU)
|
||||
> The order of joints in robot_lab cfg file `joint_names` is the same as that defined in `xxx/robot_lab/config.yaml` in this project.
|
||||
>
|
||||
> Discuss in [Github Discussion](https://github.com/fan-ziqi/rl_sar/discussions) or [Discord](https://discord.gg/vmVjkhVugU).
|
||||
|
||||
## Preparation
|
||||
|
||||
|
|
@ -32,13 +40,13 @@ This project uses `ros-noetic` (Ubuntu 20.04) and requires the installation of t
|
|||
sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller ros-noetic-joy
|
||||
```
|
||||
|
||||
Download and deploy `libtorch` at any location
|
||||
Download and deploy `libtorch` at any location (Please modify **\<YOUR_PATH\>** below to the actual path)
|
||||
|
||||
```bash
|
||||
cd /path/to/your/libtorch
|
||||
cd <YOUR_PATH>/libtorch
|
||||
wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
|
||||
unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip -d ./
|
||||
echo 'export Torch_DIR=/path/to/your/libtorch' >> ~/.bashrc
|
||||
echo 'export Torch_DIR=<YOUR_PATH>/libtorch' >> ~/.bashrc
|
||||
source ~/.bashrc
|
||||
```
|
||||
|
||||
|
|
@ -56,7 +64,7 @@ sudo apt install libtbb-dev
|
|||
|
||||
<details>
|
||||
|
||||
<summary>You can also use source code installation, click to expand</summary>
|
||||
<summary>You can also use source code installation (Click to expand)</summary>
|
||||
|
||||
Install yaml-cpp
|
||||
|
||||
|
|
@ -92,9 +100,9 @@ catkin build
|
|||
|
||||
## Running
|
||||
|
||||
In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent different environments, which can be `a1_isaacgym`, `a1_isaacsim`, `go2_isaacgym`, `gr1t1_isaacgym`, or `gr1t2_isaacgym`.
|
||||
In the following text, **\<ROBOT\>/\<CONFIG\>** is used to represent different environments, such as `a1/isaacgym` and `go2/himloco`.
|
||||
|
||||
Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`, and configure the parameters in `config.yaml`.
|
||||
Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROBOT>/<CONFIG>`, and configure the parameters in `config.yaml`.
|
||||
|
||||
### Simulation
|
||||
|
||||
|
|
@ -102,7 +110,7 @@ Open a terminal, launch the gazebo simulation environment
|
|||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
|
||||
roslaunch rl_sar gazebo_<ROBOT>.launch cfg:=<CONFIG>
|
||||
```
|
||||
|
||||
Open a new terminal, launch the control program
|
||||
|
|
@ -129,12 +137,14 @@ Gamepad Controls
|
|||
|
||||
### Real Robots
|
||||
|
||||
#### Unitree A1
|
||||
<details>
|
||||
|
||||
<summary>Unitree A1 (Click to expand)</summary>
|
||||
|
||||
Unitree A1 can be connected using both wireless and wired methods:
|
||||
|
||||
* Wireless: Connect to the Unitree starting with WIFI broadcasted by the robot **(Note: Wireless connection may lead to packet loss, disconnection, or even loss of control, please ensure safety)**
|
||||
* Wired: Use an Ethernet cable to connect any port on the computer and the robot, configure the computer IP as 192.168.123.162, and the gateway as 255.255.255.0
|
||||
* Wired: Use an Ethernet cable to connect any port on the computer and the robot, configure the computer IP as 192.168.123.162, and the netmask as 255.255.255.0
|
||||
|
||||
Open a new terminal and start the control program
|
||||
|
||||
|
|
@ -147,7 +157,11 @@ Press the **R2** button on the controller to switch the robot to the default sta
|
|||
|
||||
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.
|
||||
|
||||
#### Unitree Go2
|
||||
</details>
|
||||
|
||||
<details>
|
||||
|
||||
<summary>Unitree Go2 (Click to expand)</summary>
|
||||
|
||||
1. Connect one end of the Ethernet cable to the Go2 robot and the other end to the user's computer. Then, enable USB Ethernet on the computer and configure it. The IP address of the onboard computer on the Go2 robot is 192.168.123.161, so the computer's USB Ethernet address should be set to the same network segment as the robot. For example, enter 192.168.123.222 in the "Address" field ("222" can be replaced with another number).
|
||||
2. Use the `ifconfig` command to find the name of the network interface for the 123 network segment, such as `enxf8e43b808e06`. In the following steps, replace `<YOUR_NETWORK_INTERFACE>` with the actual network interface name.
|
||||
|
|
@ -158,6 +172,8 @@ Or press **0** on the keyboard to switch the robot to the default standing posit
|
|||
```
|
||||
4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1.
|
||||
|
||||
</details>
|
||||
|
||||
### Train the actuator network
|
||||
|
||||
Take A1 as an example below
|
||||
|
|
@ -175,10 +191,10 @@ Take A1 as an example below
|
|||
|
||||
## Add Your Robot
|
||||
|
||||
In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent your robot environment.
|
||||
In the following text, **\<ROBOT\>/\<CONFIG\>** is used to represent your robot environment.
|
||||
|
||||
1. Create a model package named `<ROBOT>_description` in the `rl_sar/src/robots` directory. Place the robot's URDF file in the `rl_sar/src/robots/<ROBOT>_description/urdf` directory and name it `<ROBOT>.urdf`. Additionally, create a joint configuration file with the namespace `<ROBOT>_gazebo` in the `rl_sar/src/robots/<ROBOT>_description/config` directory.
|
||||
2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>` directory, and create a new `config.yaml` file in this path. Refer to the `rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml` file to modify the parameters.
|
||||
2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/<ROBOT>/<CONFIG>` directory, and create a new `config.yaml` file in this path. Refer to the `rl_sar/src/rl_sar/models/a1/isaacgym/config.yaml` file to modify the parameters.
|
||||
3. Modify the `forward()` function in the code as needed to adapt to different models.
|
||||
4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory.
|
||||
5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
|
||||
|
|
|
|||
50
README_CN.md
50
README_CN.md
|
|
@ -1,5 +1,10 @@
|
|||
# rl_sar
|
||||
|
||||
[](https://ubuntu.com/)
|
||||
[](https://wiki.ros.org/noetic)
|
||||
[](https://wiki.ros.org/foxy)
|
||||
[](https://opensource.org/license/apache-2-0)
|
||||
|
||||
[English document](README.md)
|
||||
|
||||
**版本选择: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy/Humble](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
|
||||
|
|
@ -7,14 +12,17 @@
|
|||
本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real"
|
||||
|
||||
特性：
|
||||
- 支持基于IsaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，用`framework`加以区分。
|
||||
- 代码有**ROS-Noetic**和**ROS2-Foxy/Humble**两个版本
|
||||
- 代码有python和cpp两个版本，python版本可以在`src/rl_sar/scripts`中找到
|
||||
- 内置多种机器人仿真的预训练模型，包括 `Unitree-A1`、`Unitree-Go2`、`Unitree-Go2W`、`Unitree-B2`、`Unitree-B2W`、`FFTAI-GR1T1`、`FFTAI-GR1T2`、`GoldenRetriever-L4W0`、`GoldenRetriever-L4W4`；
|
||||
- 训练框架支持**IsaacGym**和**IsaacSim**，用`framework`加以区分；
|
||||
- 代码有**ROS-Noetic**和**ROS2-Foxy/Humble**两个版本；
|
||||
- 代码有**python**和**cpp**两个版本，其中python版本在`src/rl_sar/scripts`内；
|
||||
|
||||
> [!NOTE]
|
||||
> 如果你想使用IsaacLab（IsaacSim）训练策略，请使用[robot_lab](https://github.com/fan-ziqi/robot_lab)项目。
|
||||
> 如果你想使用IsaacLab（IsaacSim）训练策略，请使用 [robot_lab](https://github.com/fan-ziqi/robot_lab) 项目。
|
||||
>
|
||||
> [点击在Discord上讨论](https://discord.gg/MC9KguQHtt)
|
||||
> robot_lab配置文件中的关节顺序 `joint_names` 与本项目代码中 `xxx/robot_lab/config.yaml` 中定义的相同。
|
||||
>
|
||||
> 在 [Github Discussion](https://github.com/fan-ziqi/rl_sar/discussions) 或 [Discord](https://discord.gg/MC9KguQHtt) 中讨论
|
||||
|
||||
## 准备
|
||||
|
||||
|
|
@ -32,13 +40,13 @@ git clone https://github.com/fan-ziqi/rl_sar.git
|
|||
sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller ros-noetic-joy
|
||||
```
|
||||
|
||||
在任意位置下载并部署`libtorch`
|
||||
在任意位置下载并部署`libtorch`（请修改下面的 **\<YOUR_PATH\>** 为实际路径）
|
||||
|
||||
```bash
|
||||
cd /path/to/your/libtorch
|
||||
cd <YOUR_PATH>/libtorch
|
||||
wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
|
||||
unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip -d ./
|
||||
echo 'export Torch_DIR=/path/to/your/libtorch' >> ~/.bashrc
|
||||
echo 'export Torch_DIR=<YOUR_PATH>/libtorch' >> ~/.bashrc
|
||||
source ~/.bashrc
|
||||
```
|
||||
|
||||
|
|
@ -56,7 +64,7 @@ sudo apt install libtbb-dev
|
|||
|
||||
<details>
|
||||
|
||||
<summary>也可以使用源码安装，点击展开</summary>
|
||||
<summary>也可使用源码安装（点击展开）</summary>
|
||||
|
||||
安装yaml-cpp
|
||||
|
||||
|
|
@ -93,9 +101,9 @@ catkin build
|
|||
|
||||
## 运行
|
||||
|
||||
下文中使用 **\<ROBOT\>_\<PLATFORM\>** 代替表示不同的环境，可以是 `a1_isaacgym` 、 `a1_isaacsim` 、 `go2_isaacgym` 、 `gr1t1_isaacgym` 、 `gr1t2_isaacgym`
|
||||
下文中使用 **\<ROBOT\>/\<CONFIG\>** 代替表示不同的环境，如 `a1/isaacgym` 、 `go2/himloco`。
|
||||
|
||||
运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`中，并配置`config.yaml`中的参数。
|
||||
运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/<ROBOT>/<CONFIG>`中，并配置`config.yaml`中的参数。
|
||||
|
||||
### 仿真
|
||||
|
||||
|
|
@ -103,7 +111,7 @@ catkin build
|
|||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
|
||||
roslaunch rl_sar gazebo_<ROBOT>.launch cfg:=<CONFIG>
|
||||
```
|
||||
|
||||
打开一个新终端，启动控制程序
|
||||
|
|
@ -130,12 +138,14 @@ source devel/setup.bash
|
|||
|
||||
### 真实机器人
|
||||
|
||||
#### Unitree A1
|
||||
<details>
|
||||
|
||||
<summary>Unitree A1（点击展开）</summary>
|
||||
|
||||
与Unitree A1连接可以使用无线与有线两种方式
|
||||
|
||||
* 无线：连接机器人发出的Unitree开头的WIFI **（注意：无线连接可能会出现丢包断联甚至失控，请注意安全）**
|
||||
* 有线：用网线连接计算机和机器人的任意网口，配置计算机ip为192.168.123.162，网关255.255.255.0
|
||||
* 有线：用网线连接计算机和机器人的任意网口，配置计算机地址为192.168.123.162，子网掩码255.255.255.0
|
||||
|
||||
新建终端，启动控制程序
|
||||
|
||||
|
|
@ -148,7 +158,11 @@ rosrun rl_sar rl_real_a1
|
|||
|
||||
或者按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
|
||||
|
||||
#### Unitree Go2
|
||||
</details>
|
||||
|
||||
<details>
|
||||
|
||||
<summary>Unitree Go2（点击展开）</summary>
|
||||
|
||||
1. 用网线的一端连接Go2机器人，另一端连接用户电脑，并开启电脑的 USB Ethernet 后进行配置。机器狗机载电脑的 IP 地地址为 192.168.123.161，故需将电脑 USB Ethernet 地址设置为与机器狗同一网段，如在 Address 中输入 192.168.123.222 (“222”可以改成其他)。
|
||||
2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替
|
||||
|
|
@ -159,6 +173,8 @@ rosrun rl_sar rl_real_a1
|
|||
```
|
||||
4. Go2支持手柄与键盘控制，方法与上面a1相同
|
||||
|
||||
</details>
|
||||
|
||||
### 训练执行器网络
|
||||
|
||||
下面拿A1举例
|
||||
|
|
@ -176,10 +192,10 @@ rosrun rl_sar rl_real_a1
|
|||
|
||||
## 添加你的机器人
|
||||
|
||||
下文中使用 **\<ROBOT\>_\<PLATFORM\>** 代替表示你的机器人环境
|
||||
下文中使用 **\<ROBOT\>/\<CONFIG\>** 代替表示你的机器人环境
|
||||
|
||||
1. 在`rl_sar/src/robots`路径下创建名为`<ROBOT>_description`的模型包，将模型的urdf放到`rl_sar/src/robots/<ROBOT>_description/urdf`路径下并命名为`<ROBOT>.urdf`，并在`rl_sar/src/robots/<ROBOT>_description/config`路径下创建命名空间为`<ROBOT>_gazebo`的关节配置文件
|
||||
2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`路径下，并在此路径中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml`文件修改其中参数
|
||||
2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/<ROBOT>/<CONFIG>`路径下，并在此路径中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1/isaacgym/config.yaml`文件修改其中参数
|
||||
3. 按需修改代码中的`forward()`函数，以适配不同的模型
|
||||
4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
|
||||
5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
|
||||
|
|
|
|||
|
|
@ -43,28 +43,28 @@ link_directories(/usr/local/lib)
|
|||
include_directories(${YAML_CPP_INCLUDE_DIR})
|
||||
|
||||
# Unitree A1
|
||||
include_directories(library/unitree_legged_sdk_3.2/include)
|
||||
link_directories(library/unitree_legged_sdk_3.2/lib)
|
||||
include_directories(library/thirdparty/unitree_legged_sdk_3.2/include)
|
||||
link_directories(library/thirdparty/unitree_legged_sdk_3.2/lib)
|
||||
set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)
|
||||
|
||||
# Unitree Go2
|
||||
include_directories(library/unitree_sdk2/include)
|
||||
link_directories(library/unitree_sdk2/lib/x86_64)
|
||||
include_directories(library/unitree_sdk2/thirdparty/include)
|
||||
include_directories(library/unitree_sdk2/thirdparty/include/ddscxx)
|
||||
link_directories(library/unitree_sdk2/thirdparty/lib/x86_64)
|
||||
include_directories(library/thirdparty/unitree_sdk2/include)
|
||||
link_directories(library/thirdparty/unitree_sdk2/lib/x86_64)
|
||||
include_directories(library/thirdparty/unitree_sdk2/thirdparty/include)
|
||||
include_directories(library/thirdparty/unitree_sdk2/thirdparty/include/ddscxx)
|
||||
link_directories(library/thirdparty/unitree_sdk2/thirdparty/lib/x86_64)
|
||||
set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
library/matplotlibcpp
|
||||
library/observation_buffer
|
||||
library/rl_sdk
|
||||
library/loop
|
||||
library/core/matplotlibcpp
|
||||
library/core/observation_buffer
|
||||
library/core/rl_sdk
|
||||
library/core/loop
|
||||
)
|
||||
|
||||
add_library(rl_sdk library/rl_sdk/rl_sdk.cpp)
|
||||
add_library(rl_sdk library/core/rl_sdk/rl_sdk.cpp)
|
||||
target_link_libraries(rl_sdk "${TORCH_LIBRARIES}" Python3::Python Python3::Module TBB::tbb)
|
||||
set_property(TARGET rl_sdk PROPERTY CXX_STANDARD 14)
|
||||
find_package(Python3 COMPONENTS NumPy)
|
||||
|
|
@ -74,7 +74,7 @@ else()
|
|||
target_compile_definitions(rl_sdk WITHOUT_NUMPY)
|
||||
endif()
|
||||
|
||||
add_library(observation_buffer library/observation_buffer/observation_buffer.cpp)
|
||||
add_library(observation_buffer library/core/observation_buffer/observation_buffer.cpp)
|
||||
target_link_libraries(observation_buffer "${TORCH_LIBRARIES}")
|
||||
set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14)
|
||||
|
||||
|
|
@ -96,6 +96,13 @@ target_link_libraries(rl_real_go2
|
|||
rl_sdk observation_buffer yaml-cpp
|
||||
)
|
||||
|
||||
include_directories(library/thirdparty/l4w4_sdk)
|
||||
add_executable(rl_real_l4w4 src/rl_real_l4w4.cpp )
|
||||
target_link_libraries(rl_real_l4w4
|
||||
${catkin_LIBRARIES} -pthread
|
||||
rl_sdk observation_buffer yaml-cpp
|
||||
)
|
||||
|
||||
catkin_install_python(PROGRAMS
|
||||
scripts/rl_sim.py
|
||||
scripts/actuator_net.py
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef RL_REAL_HPP
|
||||
#define RL_REAL_HPP
|
||||
#ifndef RL_REAL_A1_HPP
|
||||
#define RL_REAL_A1_HPP
|
||||
|
||||
#include "rl_sdk.hpp"
|
||||
#include "observation_buffer.hpp"
|
||||
|
|
@ -61,8 +61,6 @@ private:
|
|||
int motiontime = 0;
|
||||
std::vector<double> mapped_joint_positions;
|
||||
std::vector<double> mapped_joint_velocities;
|
||||
int command_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
|
||||
int state_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
|
||||
};
|
||||
|
||||
#endif // RL_REAL_HPP
|
||||
#endif // RL_REAL_A1_HPP
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef RL_REAL_HPP
|
||||
#define RL_REAL_HPP
|
||||
#ifndef RL_REAL_GO2_HPP
|
||||
#define RL_REAL_GO2_HPP
|
||||
|
||||
#include "rl_sdk.hpp"
|
||||
#include "observation_buffer.hpp"
|
||||
|
|
@ -107,8 +107,6 @@ private:
|
|||
int motiontime = 0;
|
||||
std::vector<double> mapped_joint_positions;
|
||||
std::vector<double> mapped_joint_velocities;
|
||||
int command_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
|
||||
int state_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
|
||||
};
|
||||
|
||||
#endif // RL_REAL_HPP
|
||||
#endif // RL_REAL_GO2_HPP
|
||||
|
|
|
|||
|
|
@ -0,0 +1,60 @@
|
|||
/*
|
||||
* Copyright (c) 2024-2025 Ziqi Fan
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef RL_REAL_L4W4_HPP
|
||||
#define RL_REAL_L4W4_HPP
|
||||
|
||||
#include "rl_sdk.hpp"
|
||||
#include "observation_buffer.hpp"
|
||||
#include "loop.hpp"
|
||||
#include "l4w4_sdk.hpp"
|
||||
#include <csignal>
|
||||
|
||||
#include "matplotlibcpp.h"
|
||||
namespace plt = matplotlibcpp;
|
||||
|
||||
class RL_Real : public RL
|
||||
{
|
||||
public:
|
||||
RL_Real();
|
||||
~RL_Real();
|
||||
|
||||
private:
|
||||
// rl functions
|
||||
torch::Tensor Forward() override;
|
||||
void GetState(RobotState<double> *state) override;
|
||||
void SetCommand(const RobotCommand<double> *command) override;
|
||||
void RunModel();
|
||||
void RobotControl();
|
||||
|
||||
// history buffer
|
||||
ObservationBuffer history_obs_buf;
|
||||
torch::Tensor history_obs;
|
||||
|
||||
// loop
|
||||
std::shared_ptr<LoopFunc> loop_keyboard;
|
||||
std::shared_ptr<LoopFunc> loop_control;
|
||||
std::shared_ptr<LoopFunc> loop_rl;
|
||||
std::shared_ptr<LoopFunc> loop_plot;
|
||||
|
||||
// plot
|
||||
const int plot_size = 100;
|
||||
std::vector<int> plot_t;
|
||||
std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
|
||||
void Plot();
|
||||
|
||||
// l4w4 interface
|
||||
L4W4SDK l4w4_sdk;
|
||||
LowCmd l4w4_low_command = {0};
|
||||
LowState l4w4_low_state = {0};
|
||||
xRockerBtnDataStruct unitree_joy;
|
||||
|
||||
// others
|
||||
int motiontime = 0;
|
||||
std::vector<double> mapped_joint_positions;
|
||||
std::vector<double> mapped_joint_velocities;
|
||||
};
|
||||
|
||||
#endif // RL_REAL_L4W4_HPP
|
||||
|
|
@ -1,7 +1,9 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="a1"/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
|
|
@ -1,7 +1,9 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="a1"/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)_isaacsim"/>
|
||||
<arg name="rname" default="b2"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
|
|
@ -32,7 +34,7 @@
|
|||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<!-- Set trunk and joint positions at startup -->
|
||||
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
|
||||
args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/>
|
||||
args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description"/>
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="b2w"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
<arg name="dollar" value="$"/>
|
||||
|
||||
<arg name="paused" default="true"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||
<arg name="user_debug" default="false"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<param name="robot_description" textfile="$(find b2w_description)/urdf/b2w_description.urdf"/>
|
||||
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<!-- Set trunk and joint positions at startup -->
|
||||
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
|
||||
args="-urdf -z 1.2 -model $(arg rname)_gazebo -param robot_description"/>
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
|
||||
output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
|
||||
FL_hip_controller FL_thigh_controller FL_calf_controller FL_foot_controller
|
||||
FR_hip_controller FR_thigh_controller FR_calf_controller FR_foot_controller
|
||||
RL_hip_controller RL_thigh_controller RL_calf_controller RL_foot_controller
|
||||
RR_hip_controller RR_thigh_controller RR_calf_controller RR_foot_controller "/>
|
||||
|
||||
<!-- convert joint states to TF transforms for rviz, etc -->
|
||||
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
|
||||
respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
|
||||
</node>
|
||||
|
||||
<!-- Load joy node -->
|
||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,7 +1,9 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="go2"/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="go2w"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
<arg name="dollar" value="$"/>
|
||||
|
||||
<arg name="paused" default="true"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||
<arg name="user_debug" default="false"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<param name="robot_description" textfile="$(find go2w_description)/urdf/go2w_description.urdf"/>
|
||||
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<!-- Set trunk and joint positions at startup -->
|
||||
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
|
||||
args="-urdf -z 0.8 -model $(arg rname)_gazebo -param robot_description"/>
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
|
||||
output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
|
||||
FL_hip_controller FL_thigh_controller FL_calf_controller FL_foot_controller
|
||||
FR_hip_controller FR_thigh_controller FR_calf_controller FR_foot_controller
|
||||
RL_hip_controller RL_thigh_controller RL_calf_controller RL_foot_controller
|
||||
RR_hip_controller RR_thigh_controller RR_calf_controller RR_foot_controller "/>
|
||||
|
||||
<!-- convert joint states to TF transforms for rviz, etc -->
|
||||
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
|
||||
respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
|
||||
</node>
|
||||
|
||||
<!-- Load joy node -->
|
||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,7 +1,9 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="gr1t1"/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
|
|
@ -1,7 +1,9 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="gr1t2"/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
<launch>
|
||||
<arg name="wname" default="stairs"/>
|
||||
<arg name="rname" default="l4w4"/>
|
||||
<arg name="cfg" default=""/>
|
||||
<param name="robot_name" type="str" value="$(arg rname)"/>
|
||||
<param name="config_name" type="str" value="$(arg cfg)"/>
|
||||
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
|
||||
<param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
|
||||
<arg name="robot_path" value="(find $(arg rname)_description)"/>
|
||||
<arg name="dollar" value="$"/>
|
||||
|
||||
<arg name="paused" default="true"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||
<arg name="user_debug" default="false"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<param name="robot_description" textfile="$(find l4w4_description)/urdf/l4w4.urdf"/>
|
||||
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<!-- Set trunk and joint positions at startup -->
|
||||
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
|
||||
args="-urdf -z 1.0 -model $(arg rname)_gazebo -param robot_description"/>
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
|
||||
output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
|
||||
FL_hip_controller FL_thigh_controller FL_calf_controller FL_foot_controller
|
||||
FR_hip_controller FR_thigh_controller FR_calf_controller FR_foot_controller
|
||||
RL_hip_controller RL_thigh_controller RL_calf_controller RL_foot_controller
|
||||
RR_hip_controller RR_thigh_controller RR_calf_controller RR_foot_controller "/>
|
||||
|
||||
<!-- convert joint states to TF transforms for rviz, etc -->
|
||||
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
|
||||
respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
|
||||
</node>
|
||||
|
||||
<!-- Load joy node -->
|
||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -38,10 +38,18 @@ void ObservationBuffer::insert(torch::Tensor new_obs)
|
|||
obs_buf.index({torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(-num_obs, torch::indexing::None)}) = new_obs;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Gets history of observations indexed by obs_ids.
|
||||
*
|
||||
* @param obs_ids An array of integers with which to index the desired
|
||||
* observations, where 0 is the latest observation and
|
||||
* include_history_steps - 1 is the oldest observation.
|
||||
* @return A torch::Tensor containing the concatenated observations.
|
||||
*/
|
||||
torch::Tensor ObservationBuffer::get_obs_vec(std::vector<int> obs_ids)
|
||||
{
|
||||
std::vector<torch::Tensor> obs;
|
||||
for (int i = obs_ids.size() - 1; i >= 0; --i)
|
||||
for (int i = 0; i < obs_ids.size(); ++i)
|
||||
{
|
||||
int obs_id = obs_ids[i];
|
||||
int slice_idx = include_history_steps - obs_id - 1;
|
||||
|
|
@ -102,17 +102,18 @@ void RL::InitControl()
|
|||
|
||||
void RL::ComputeOutput(const torch::Tensor &actions, torch::Tensor &output_dof_pos, torch::Tensor &output_dof_vel, torch::Tensor &output_dof_tau)
|
||||
{
|
||||
torch::Tensor joint_actions_scaled = actions * this->params.action_scale;
|
||||
torch::Tensor wheel_actions_scaled = torch::zeros({1, this->params.num_of_dofs});
|
||||
torch::Tensor actions_scaled = actions * this->params.action_scale;
|
||||
torch::Tensor pos_actions_scaled = actions_scaled;
|
||||
torch::Tensor vel_actions_scaled = torch::zeros_like(actions);
|
||||
for (int i : this->params.wheel_indices)
|
||||
{
|
||||
joint_actions_scaled[0][i] = 0.0;
|
||||
wheel_actions_scaled[0][i] = actions[0][i] * this->params.action_scale_wheel;
|
||||
pos_actions_scaled[0][i] = 0.0;
|
||||
vel_actions_scaled[0][i] = actions[0][i];
|
||||
}
|
||||
torch::Tensor actions_scaled = joint_actions_scaled + wheel_actions_scaled;
|
||||
output_dof_pos = joint_actions_scaled + this->params.default_dof_pos;
|
||||
output_dof_vel = wheel_actions_scaled;
|
||||
output_dof_tau = this->params.rl_kp * (actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.rl_kd * this->obs.dof_vel;
|
||||
torch::Tensor all_actions_scaled = pos_actions_scaled + vel_actions_scaled;
|
||||
output_dof_pos = pos_actions_scaled + this->params.default_dof_pos;
|
||||
output_dof_vel = vel_actions_scaled;
|
||||
output_dof_tau = this->params.rl_kp * (all_actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.rl_kd * this->obs.dof_vel;
|
||||
output_dof_tau = torch::clamp(output_dof_tau, -(this->params.torque_limits), this->params.torque_limits);
|
||||
}
|
||||
|
||||
|
|
@ -458,45 +459,14 @@ std::vector<T> ReadVectorFromYaml(const YAML::Node &node)
|
|||
return values;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
std::vector<T> ReadVectorFromYaml(const YAML::Node &node, const std::string &framework, const int &rows, const int &cols)
|
||||
void RL::ReadYaml(std::string robot_path)
|
||||
{
|
||||
std::vector<T> values;
|
||||
for (const auto &val : node)
|
||||
{
|
||||
values.push_back(val.as<T>());
|
||||
}
|
||||
|
||||
if (framework == "isaacsim")
|
||||
{
|
||||
std::vector<T> transposed_values(cols * rows);
|
||||
for (int r = 0; r < rows; ++r)
|
||||
{
|
||||
for (int c = 0; c < cols; ++c)
|
||||
{
|
||||
transposed_values[c * rows + r] = values[r * cols + c];
|
||||
}
|
||||
}
|
||||
return transposed_values;
|
||||
}
|
||||
else if (framework == "isaacgym")
|
||||
{
|
||||
return values;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw std::invalid_argument("Unsupported framework: " + framework);
|
||||
}
|
||||
}
|
||||
|
||||
void RL::ReadYaml(std::string robot_name)
|
||||
{
|
||||
// The config file is located at "rl_sar/src/rl_sar/models/<robot_name>/config.yaml"
|
||||
std::string config_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/config.yaml";
|
||||
// The config file is located at "rl_sar/src/rl_sar/models/<robot_path/config.yaml"
|
||||
std::string config_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/config.yaml";
|
||||
YAML::Node config;
|
||||
try
|
||||
{
|
||||
config = YAML::LoadFile(config_path)[robot_name];
|
||||
config = YAML::LoadFile(config_path)[robot_path];
|
||||
}
|
||||
catch (YAML::BadFile &e)
|
||||
{
|
||||
|
|
@ -506,8 +476,6 @@ void RL::ReadYaml(std::string robot_name)
|
|||
|
||||
this->params.model_name = config["model_name"].as<std::string>();
|
||||
this->params.framework = config["framework"].as<std::string>();
|
||||
int rows = config["rows"].as<int>();
|
||||
int cols = config["cols"].as<int>();
|
||||
this->params.dt = config["dt"].as<double>();
|
||||
this->params.decimation = config["decimation"].as<int>();
|
||||
this->params.num_observations = config["num_observations"].as<int>();
|
||||
|
|
@ -528,13 +496,10 @@ void RL::ReadYaml(std::string robot_name)
|
|||
}
|
||||
else
|
||||
{
|
||||
this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"])).view({1, -1});
|
||||
this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"])).view({1, -1});
|
||||
}
|
||||
this->params.action_scale = config["action_scale"].as<double>();
|
||||
this->params.hip_scale_reduction = config["hip_scale_reduction"].as<double>();
|
||||
this->params.hip_scale_reduction_indices = ReadVectorFromYaml<int>(config["hip_scale_reduction_indices"]);
|
||||
this->params.action_scale_wheel = config["action_scale_wheel"].as<double>();
|
||||
this->params.action_scale = torch::tensor(ReadVectorFromYaml<double>(config["action_scale"])).view({1, -1});
|
||||
this->params.wheel_indices = ReadVectorFromYaml<int>(config["wheel_indices"]);
|
||||
this->params.num_of_dofs = config["num_of_dofs"].as<int>();
|
||||
this->params.lin_vel_scale = config["lin_vel_scale"].as<double>();
|
||||
|
|
@ -543,13 +508,15 @@ void RL::ReadYaml(std::string robot_name)
|
|||
this->params.dof_vel_scale = config["dof_vel_scale"].as<double>();
|
||||
this->params.commands_scale = torch::tensor(ReadVectorFromYaml<double>(config["commands_scale"])).view({1, -1});
|
||||
// this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
|
||||
this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"], this->params.framework, rows, cols)).view({1, -1});
|
||||
this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"], this->params.framework, rows, cols);
|
||||
this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"])).view({1, -1});
|
||||
this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"])).view({1, -1});
|
||||
this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"])).view({1, -1});
|
||||
this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"])).view({1, -1});
|
||||
this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"])).view({1, -1});
|
||||
this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"])).view({1, -1});
|
||||
this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"]);
|
||||
this->params.command_mapping = ReadVectorFromYaml<int>(config["command_mapping"]);
|
||||
this->params.state_mapping = ReadVectorFromYaml<int>(config["state_mapping"]);
|
||||
}
|
||||
|
||||
void RL::CSVInit(std::string robot_name)
|
||||
|
|
@ -87,10 +87,7 @@ struct ModelParams
|
|||
std::vector<int> observations_history;
|
||||
double damping;
|
||||
double stiffness;
|
||||
double action_scale;
|
||||
double hip_scale_reduction;
|
||||
std::vector<int> hip_scale_reduction_indices;
|
||||
double action_scale_wheel;
|
||||
torch::Tensor action_scale;
|
||||
std::vector<int> wheel_indices;
|
||||
int num_of_dofs;
|
||||
double lin_vel_scale;
|
||||
|
|
@ -108,6 +105,8 @@ struct ModelParams
|
|||
torch::Tensor commands_scale;
|
||||
torch::Tensor default_dof_pos;
|
||||
std::vector<std::string> joint_controller_names;
|
||||
std::vector<int> command_mapping;
|
||||
std::vector<int> state_mapping;
|
||||
};
|
||||
|
||||
struct Observations
|
||||
|
|
@ -152,7 +151,7 @@ public:
|
|||
torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string &framework);
|
||||
|
||||
// yaml params
|
||||
void ReadYaml(std::string robot_name);
|
||||
void ReadYaml(std::string robot_path);
|
||||
|
||||
// csv logger
|
||||
std::string csv_filename;
|
||||
|
|
@ -164,7 +163,7 @@ public:
|
|||
void KeyboardInterface();
|
||||
|
||||
// others
|
||||
std::string robot_name;
|
||||
std::string robot_name, config_name;
|
||||
STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING
|
||||
bool simulation_running = false;
|
||||
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#ifndef L4W4_SDK_COMM_H
|
||||
#define L4W4_SDK_COMM_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} Vector3;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float quaternion[4]; // quaternion, normalized, (w,x,y,z)
|
||||
float gyroscope[3]; // angular velocity （unit: rad/s)
|
||||
float accelerometer[3]; // m/(s2)
|
||||
float rpy[3]; // euler angle（unit: rad)
|
||||
int8_t temperature;
|
||||
} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode; // motor working mode
|
||||
float q; // current angle (unit: radian)
|
||||
float dq; // current velocity (unit: radian/second)
|
||||
float ddq; // current acc (unit: radian/second*second)
|
||||
float tauEst; // current estimated output torque (unit: N.m)
|
||||
float q_raw; // current angle (unit: radian)
|
||||
float dq_raw; // current velocity (unit: radian/second)
|
||||
float ddq_raw;
|
||||
int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
|
||||
uint32_t reserve[2];
|
||||
} MotorState; // motor feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode; // desired working mode
|
||||
float q; // desired angle (unit: radian)
|
||||
float dq; // desired velocity (unit: radian/second)
|
||||
float tau; // desired output torque (unit: N.m)
|
||||
float Kp; // desired position stiffness (unit: N.m/rad )
|
||||
float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
|
||||
uint32_t reserve[3];
|
||||
} MotorCmd; // motor control
|
||||
|
||||
typedef struct
|
||||
{
|
||||
IMU imu;
|
||||
MotorState motorState[20];
|
||||
uint8_t wirelessRemote[40]; // wireless commands
|
||||
} LowState; // low level feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
MotorCmd motorCmd[20];
|
||||
uint8_t wirelessRemote[40];
|
||||
} LowCmd; // low level control
|
||||
|
||||
#endif // L4W4_SDK_COMM_H
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
#ifndef L4W4_SDK_JOYSTICK_H
|
||||
#define L4W4_SDK_JOYSTICK_H
|
||||
|
||||
#include <stdint.h>
|
||||
// 16b
|
||||
typedef union {
|
||||
struct {
|
||||
uint8_t R1 :1;
|
||||
uint8_t L1 :1;
|
||||
uint8_t start :1;
|
||||
uint8_t select :1;
|
||||
uint8_t R2 :1;
|
||||
uint8_t L2 :1;
|
||||
uint8_t F1 :1;
|
||||
uint8_t F2 :1;
|
||||
uint8_t A :1;
|
||||
uint8_t B :1;
|
||||
uint8_t X :1;
|
||||
uint8_t Y :1;
|
||||
uint8_t up :1;
|
||||
uint8_t right :1;
|
||||
uint8_t down :1;
|
||||
uint8_t left :1;
|
||||
} components;
|
||||
uint16_t value;
|
||||
} xKeySwitchUnion;
|
||||
|
||||
// 40 Byte (now used 24B)
|
||||
typedef struct {
|
||||
uint8_t head[2];
|
||||
xKeySwitchUnion btn;
|
||||
float lx;
|
||||
float rx;
|
||||
float ry;
|
||||
float L2;
|
||||
float ly;
|
||||
|
||||
uint8_t idle[16];
|
||||
} xRockerBtnDataStruct;
|
||||
|
||||
#endif // L4W4_SDK_JOYSTICK_H
|
||||
|
|
@ -0,0 +1,534 @@
|
|||
#ifndef L4W4_SDK_HPP
|
||||
#define L4W4_SDK_HPP
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <netinet/in.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/stat.h>
|
||||
#include <string.h>
|
||||
#include <arpa/inet.h>
|
||||
#include "joystick.h"
|
||||
#include "comm.h"
|
||||
|
||||
class L4W4SDK
|
||||
{
|
||||
private:
|
||||
float tmp_time_from_mcu = -1;
|
||||
int show_rc = 0;
|
||||
int ctr = 0;
|
||||
float t0 = -1;
|
||||
int n_cpu = 0;
|
||||
int n_run = 0;
|
||||
|
||||
float read_float(unsigned char *buff, int *idx);
|
||||
float read_from1byte(unsigned char *buff, int *idx, float v_min, float v_max);
|
||||
float read_from2bytes(unsigned char *buff, int *idx, float v_min, float v_max);
|
||||
void write_into2bytes(float value, unsigned char *buff, int *idx, float v_min, float v_max);
|
||||
float read_byte(unsigned char *buff, int *idx);
|
||||
float Vector3_Dot(Vector3 v1, Vector3 v2);
|
||||
Vector3 Vector3_Cross(Vector3 u, Vector3 v);
|
||||
float Sign(float value);
|
||||
float Vector3_invSqrt(Vector3 v);
|
||||
Vector3 Vector3_Direction(Vector3 v);
|
||||
float Clamp(float value, float min, float max);
|
||||
float Angle_vA_2_vB(Vector3 vA, Vector3 vB, Vector3 axle);
|
||||
void Vector3_Normalize(Vector3 *v);
|
||||
Vector3 Quaternion_Transform(float vx, float vy, float vz, float qx, float qy, float qz, float qw);
|
||||
public:
|
||||
L4W4SDK() {};
|
||||
~L4W4SDK() {};
|
||||
int client_socket;
|
||||
struct sockaddr_in server_addr;
|
||||
int recv_len = 0;
|
||||
int ex_send_recv = -1;
|
||||
unsigned char sent_buff[256];
|
||||
unsigned char recv_buff[512];
|
||||
void InitUDP();
|
||||
void AnalyzeUDP(unsigned char *recv_buff, LowState &lowState);
|
||||
void SendUDP(LowCmd &lowCmd);
|
||||
void PrintMCU(int running_state);
|
||||
void InitCmdData(LowCmd& cmd);
|
||||
};
|
||||
|
||||
void L4W4SDK::InitCmdData(LowCmd& cmd)
|
||||
{
|
||||
for (int i = 0; i < 20; ++i) {
|
||||
cmd.motorCmd[i].mode = 0;
|
||||
cmd.motorCmd[i].q = 0.0;
|
||||
cmd.motorCmd[i].dq = 0.0;
|
||||
cmd.motorCmd[i].tau = 0.0;
|
||||
cmd.motorCmd[i].Kp = 0.0;
|
||||
cmd.motorCmd[i].Kd = 0.0;
|
||||
}
|
||||
memset(cmd.wirelessRemote, 0, sizeof(cmd.wirelessRemote));
|
||||
}
|
||||
|
||||
void L4W4SDK::PrintMCU(int running_state)
|
||||
{
|
||||
float t1 = tmp_time_from_mcu;
|
||||
if(t1 < t0)
|
||||
t0 = t1;
|
||||
|
||||
float ms = (t1 - t0) * 1000.0;
|
||||
n_cpu++;
|
||||
n_run++;
|
||||
|
||||
if(ms > 1000)
|
||||
{
|
||||
std::cout<<"mcu time = "<< tmp_time_from_mcu<<", n_cpu = "<<n_cpu<<", runningState= "<< running_state<<std::endl;
|
||||
t0 = t1;
|
||||
n_cpu = 0;
|
||||
n_run = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(n_run > 50)
|
||||
{
|
||||
std::cout<<" dull ms= "<<ms<<", n_cpu = "<<n_cpu<<", runningState= "<< running_state<<std::endl;
|
||||
n_run = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void L4W4SDK::SendUDP(LowCmd &lowCmd)
|
||||
{
|
||||
int idx = 0;
|
||||
|
||||
// front right
|
||||
write_into2bytes(lowCmd.motorCmd[4].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[4].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[4].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[5].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[5].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[5].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[6].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[6].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[6].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[7].dq, sent_buff, &idx, -200, 200 );
|
||||
write_into2bytes(lowCmd.motorCmd[7].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[7].Kd, sent_buff, &idx, 0, 1000 );
|
||||
|
||||
// rear right
|
||||
write_into2bytes(lowCmd.motorCmd[12].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[12].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[12].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[13].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[13].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[13].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[14].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[14].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[14].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[15].dq, sent_buff, &idx, -200, 200 );
|
||||
write_into2bytes(lowCmd.motorCmd[15].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[15].Kd, sent_buff, &idx, 0, 1000 );
|
||||
|
||||
// rear left
|
||||
write_into2bytes(lowCmd.motorCmd[8].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[8].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[8].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[9].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[9].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[9].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[10].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[10].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[10].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[11].dq, sent_buff, &idx, -200, 200 );
|
||||
write_into2bytes(lowCmd.motorCmd[11].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[11].Kd, sent_buff, &idx, 0, 1000 );
|
||||
|
||||
// front left
|
||||
write_into2bytes(lowCmd.motorCmd[0].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[0].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[0].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[1].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[1].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[1].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[2].q, sent_buff, &idx, -3.2, 3.2 );
|
||||
write_into2bytes(lowCmd.motorCmd[2].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[2].Kd, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(-lowCmd.motorCmd[3].dq, sent_buff, &idx, -200, 200 );
|
||||
write_into2bytes(lowCmd.motorCmd[3].Kp, sent_buff, &idx, 0, 1000 );
|
||||
write_into2bytes(lowCmd.motorCmd[3].Kd, sent_buff, &idx, 0, 1000 );
|
||||
|
||||
int sss = sendto(client_socket, sent_buff, idx, 0, (const sockaddr*)& server_addr, sizeof(server_addr));
|
||||
}
|
||||
|
||||
void L4W4SDK::InitUDP()
|
||||
{
|
||||
struct sockaddr_in client_addr;
|
||||
client_addr.sin_family = AF_INET;
|
||||
client_addr.sin_addr.s_addr = htons(INADDR_ANY);//INADDR_ANY表示自动获取本机地址 inet_addr("192.168.1.102");
|
||||
client_addr.sin_port = htons(0); //0表示让系统自动分配一个空闲端口
|
||||
client_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||||
if(client_socket < 0)
|
||||
{
|
||||
std::cout <<"socket error" << std::endl;
|
||||
perror("socket error");
|
||||
exit(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout<<"socket created ("<<client_socket<<")"<<std::endl;
|
||||
}
|
||||
|
||||
int ret = bind(client_socket, (struct sockaddr*)&client_addr, sizeof(client_addr));
|
||||
if(ret >= 0)
|
||||
{
|
||||
std::cout << "udp bind success (" << ret << ")" <<std::endl;
|
||||
std::cout << "my ip = " << inet_ntoa(client_addr.sin_addr) << std::endl;
|
||||
std::cout << "my port = " << client_addr.sin_port << std::endl;
|
||||
}
|
||||
else
|
||||
std::cout << "udp bind failed (" << ret << ")" <<std::endl;
|
||||
|
||||
|
||||
server_addr.sin_family = AF_INET;
|
||||
server_addr.sin_port = htons(777);
|
||||
server_addr.sin_addr.s_addr = inet_addr("192.168.1.101");
|
||||
|
||||
|
||||
}
|
||||
|
||||
float L4W4SDK::read_float(unsigned char *buff, int *idx)
|
||||
{
|
||||
float v = 0;
|
||||
*((unsigned char*)&v + 0) = buff[*idx]; (*idx)++;
|
||||
*((unsigned char*)&v + 1) = buff[*idx]; (*idx)++;
|
||||
*((unsigned char*)&v + 2) = buff[*idx]; (*idx)++;
|
||||
*((unsigned char*)&v + 3) = buff[*idx]; (*idx)++;
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
float L4W4SDK::read_from1byte(unsigned char *buff, int *idx, float v_min, float v_max)
|
||||
{
|
||||
float v = (float)buff[*idx] / 255.0 * (v_max - v_min) + v_min;
|
||||
(*idx)++;
|
||||
return v;
|
||||
}
|
||||
|
||||
float L4W4SDK::read_from2bytes(unsigned char *buff, int *idx, float v_min, float v_max)
|
||||
{
|
||||
unsigned int v = 0;
|
||||
*((unsigned char*)&v + 0) = buff[*idx]; (*idx)++;
|
||||
*((unsigned char*)&v + 1) = buff[*idx]; (*idx)++;
|
||||
|
||||
return v / 65535.0 * (v_max - v_min) + v_min;
|
||||
}
|
||||
|
||||
void L4W4SDK::write_into2bytes(float value, unsigned char *buff, int *idx, float v_min, float v_max)
|
||||
{
|
||||
ushort v16 = (value - v_min) / (v_max - v_min) * 65535;
|
||||
buff[*idx] = (v16>>8) & 0xff;
|
||||
(*idx)++;
|
||||
buff[*idx] = v16 & 0xff;
|
||||
(*idx)++;
|
||||
}
|
||||
|
||||
float L4W4SDK::read_byte(unsigned char *buff, int *idx)
|
||||
{
|
||||
float v = (float)buff[*idx];
|
||||
(*idx)++;
|
||||
return v;
|
||||
}
|
||||
|
||||
float L4W4SDK::Vector3_Dot(Vector3 v1, Vector3 v2)
|
||||
{
|
||||
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
|
||||
}
|
||||
|
||||
Vector3 L4W4SDK::Vector3_Cross(Vector3 u, Vector3 v)
|
||||
{
|
||||
Vector3 vn;
|
||||
vn.x = u.y * v.z - u.z * v.y;
|
||||
vn.y = u.z * v.x - u.x * v.z;
|
||||
vn.z = u.x * v.y - u.y * v.x;
|
||||
return vn;
|
||||
}
|
||||
|
||||
float L4W4SDK::Sign(float value)
|
||||
{
|
||||
if(value >= 0)
|
||||
return 1;
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
|
||||
float L4W4SDK::Vector3_invSqrt(Vector3 v)
|
||||
{
|
||||
return 1.0 / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
|
||||
}
|
||||
|
||||
Vector3 L4W4SDK::Vector3_Direction(Vector3 v)
|
||||
{
|
||||
float invSqrt = Vector3_invSqrt(v);
|
||||
Vector3 dv = v;
|
||||
dv.x *= invSqrt;
|
||||
dv.y *= invSqrt;
|
||||
dv.z *= invSqrt;
|
||||
|
||||
return dv;
|
||||
}
|
||||
|
||||
float L4W4SDK::Clamp(float value, float min, float max)
|
||||
{
|
||||
if(min > max)
|
||||
{
|
||||
float tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
}
|
||||
|
||||
if(value < min) return min;
|
||||
else if(value > max) return max;
|
||||
else return value;
|
||||
}
|
||||
|
||||
float L4W4SDK::Angle_vA_2_vB(Vector3 vA, Vector3 vB, Vector3 axle)
|
||||
{
|
||||
Vector3 vA_dir = Vector3_Direction(vA);
|
||||
Vector3 vB_dir = Vector3_Direction(vB);
|
||||
Vector3 axle_dir = Vector3_Direction(axle);
|
||||
|
||||
float sinA = Clamp( Vector3_Dot(axle_dir, Vector3_Cross(vA_dir, vB_dir) ), -1, 1);
|
||||
float angle_raw = asinf(sinA);
|
||||
|
||||
float angle = Vector3_Dot(vA_dir, vB_dir) > 0 ? angle_raw : Sign(angle_raw) * M_PI - angle_raw;
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
void L4W4SDK::Vector3_Normalize(Vector3 *v)
|
||||
{
|
||||
float invSqrt = Vector3_invSqrt(*v);
|
||||
v->x *= invSqrt;
|
||||
v->y *= invSqrt;
|
||||
v->z *= invSqrt;
|
||||
}
|
||||
Vector3 L4W4SDK::Quaternion_Transform(float vx, float vy, float vz, float qx, float qy, float qz, float qw)
|
||||
{
|
||||
float dot_u_v = qx * vx + qy * vy + qz * vz;
|
||||
float dot_u_u = qx * qx + qy * qy + qz * qz;
|
||||
float x_of_uXv = qy * vz - qz * vy;
|
||||
float y_of_uXv = qz * vx - qx * vz;
|
||||
float z_of_uXv = qx * vy - qy * vx;
|
||||
float k1 = 2.0 * dot_u_v;
|
||||
float k2 = qw * qw - dot_u_u;
|
||||
float k3 = 2.0 * qw;
|
||||
|
||||
Vector3 vout;
|
||||
vout.x = k1 * qx + k2 * vx + k3 * x_of_uXv;
|
||||
vout.y = k1 * qy + k2 * vy + k3 * y_of_uXv;
|
||||
vout.z = k1 * qz + k2 * vz + k3 * z_of_uXv;
|
||||
return vout;
|
||||
}
|
||||
|
||||
|
||||
void L4W4SDK::AnalyzeUDP(unsigned char *recv_buff, LowState &lowState)
|
||||
{
|
||||
int idx = 2;
|
||||
tmp_time_from_mcu = read_float(recv_buff, &idx);
|
||||
float BatteryVoltage = read_from1byte(recv_buff, &idx, 20, 60);
|
||||
float MCUTemperature = read_byte(recv_buff, &idx);
|
||||
|
||||
float acc_lx = read_float(recv_buff, &idx);
|
||||
float acc_ly = read_float(recv_buff, &idx);
|
||||
float acc_lz = read_float(recv_buff, &idx);
|
||||
float omega_lx = read_float(recv_buff, &idx);
|
||||
float omega_ly = read_float(recv_buff, &idx);
|
||||
float omega_lz = read_float(recv_buff, &idx);
|
||||
float orientation_x = read_float(recv_buff, &idx);
|
||||
float orientation_y = read_float(recv_buff, &idx);
|
||||
float orientation_z = read_float(recv_buff, &idx);
|
||||
float orientation_w = read_float(recv_buff, &idx);
|
||||
|
||||
|
||||
|
||||
Vector3 robot_up_w = Quaternion_Transform(0, 1, 0, orientation_x, orientation_y, orientation_z, orientation_w);
|
||||
Vector3 current_trunk_front = Quaternion_Transform(1, 0, 0, orientation_x, orientation_y, orientation_z, orientation_w);
|
||||
Vector3 current_trunk_right = Quaternion_Transform(0, 0, 1, orientation_x, orientation_y, orientation_z, orientation_w);
|
||||
Vector3 trunk_hori_front = current_trunk_front;
|
||||
trunk_hori_front.y = 0;
|
||||
Vector3_Normalize(&trunk_hori_front);
|
||||
Vector3 trunk_hori_right = Vector3_Cross(trunk_hori_front, {0,1,0});
|
||||
float r2d = 180.0f / M_PI;
|
||||
|
||||
|
||||
float robot_yaw_deg = Angle_vA_2_vB({1,0,0}, trunk_hori_front, {0,1,0}) * r2d;
|
||||
float robot_pitch_deg = Angle_vA_2_vB(trunk_hori_front, current_trunk_front, trunk_hori_right) * r2d;
|
||||
float robot_roll_deg = Angle_vA_2_vB(trunk_hori_right, current_trunk_right, current_trunk_front) * r2d;
|
||||
|
||||
|
||||
read_byte(recv_buff, &idx);
|
||||
read_byte(recv_buff, &idx);
|
||||
|
||||
xRockerBtnDataStruct *rockerBtn = (xRockerBtnDataStruct*)(&(lowState.wirelessRemote));
|
||||
|
||||
unsigned char key1 = recv_buff[idx]; idx++;
|
||||
rockerBtn->btn.components.R1 = (key1 & 0x80) >> 7;
|
||||
rockerBtn->btn.components.L1 = (key1 & 0x40) >> 6;
|
||||
rockerBtn->btn.components.start = (key1 & 0x20) >> 5;
|
||||
rockerBtn->btn.components.select = (key1 & 0x10) >> 4;
|
||||
rockerBtn->btn.components.R2 = (key1 & 0x08) >> 3;
|
||||
rockerBtn->btn.components.L2 = (key1 & 0x04) >> 2;
|
||||
rockerBtn->btn.components.F1 = (key1 & 0x02) >> 1;
|
||||
rockerBtn->btn.components.F2 = (key1 & 0x01) >> 0;
|
||||
|
||||
unsigned char key2 = recv_buff[idx]; idx++;
|
||||
rockerBtn->btn.components.A = (key2 & 0x80) >> 7;
|
||||
rockerBtn->btn.components.B = (key2 & 0x40) >> 6;
|
||||
rockerBtn->btn.components.X = (key2 & 0x20) >> 5;
|
||||
rockerBtn->btn.components.Y = (key2 & 0x10) >> 4;
|
||||
rockerBtn->btn.components.up = (key2 & 0x08) >> 3;
|
||||
rockerBtn->btn.components.right = (key2 & 0x04) >> 2;
|
||||
rockerBtn->btn.components.down = (key2 & 0x02) >> 1;
|
||||
rockerBtn->btn.components.left = (key2 & 0x01) >> 0;
|
||||
|
||||
|
||||
rockerBtn->lx = read_float(recv_buff, &idx);
|
||||
rockerBtn->rx = read_float(recv_buff, &idx);
|
||||
rockerBtn->ly = read_float(recv_buff, &idx);
|
||||
rockerBtn->L2 = read_float(recv_buff, &idx);
|
||||
rockerBtn->ry = read_float(recv_buff, &idx);
|
||||
|
||||
if(rockerBtn->btn.components.F1)
|
||||
show_rc = 1;
|
||||
else
|
||||
show_rc = 0;
|
||||
|
||||
idx+=4*4;
|
||||
|
||||
float motor_leg1_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg1_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg1_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI);
|
||||
float motor_leg1_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg1_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg1_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
|
||||
float motor_leg2_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg2_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg2_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI);
|
||||
float motor_leg2_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg2_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg2_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
|
||||
float motor_leg3_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg3_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg3_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI);
|
||||
float motor_leg3_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg3_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg3_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
|
||||
float motor_leg4_j0 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg4_j0_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg4_j1 = read_from2bytes(recv_buff, &idx, -1.3*M_PI, 0.7*M_PI);
|
||||
float motor_leg4_j1_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
float motor_leg4_j2 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg4_j2_dot = read_from2bytes(recv_buff, &idx, -33, 33);
|
||||
|
||||
float motor_leg1_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg1_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200);
|
||||
float motor_leg2_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg2_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200);
|
||||
float motor_leg3_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg3_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200);
|
||||
float motor_leg4_j3 = read_from2bytes(recv_buff, &idx, -M_PI, M_PI);
|
||||
float motor_leg4_j3_dot = read_from2bytes(recv_buff, &idx, -200, 200);
|
||||
|
||||
lowState.imu.quaternion[0] = orientation_w;
|
||||
lowState.imu.quaternion[1] = orientation_x;
|
||||
lowState.imu.quaternion[2] = -orientation_z;
|
||||
lowState.imu.quaternion[3] = orientation_y;
|
||||
|
||||
lowState.imu.gyroscope[0] = omega_lx;
|
||||
lowState.imu.gyroscope[1] = -omega_lz;
|
||||
lowState.imu.gyroscope[2] = omega_ly;
|
||||
lowState.imu.accelerometer[0] = acc_lx;
|
||||
lowState.imu.accelerometer[1] = -acc_lz;
|
||||
lowState.imu.accelerometer[2] = acc_ly;
|
||||
|
||||
// leg4 leg1 leg3 leg2
|
||||
// hip thigh shank wheel
|
||||
|
||||
lowState.motorState[0].q = lowState.motorState[0].q_raw = motor_leg4_j0;
|
||||
lowState.motorState[0].dq = lowState.motorState[0].dq_raw = motor_leg4_j0_dot;
|
||||
lowState.motorState[0].ddq = lowState.motorState[0].ddq_raw = 0;
|
||||
lowState.motorState[1].q = lowState.motorState[1].q_raw = -motor_leg4_j1;
|
||||
lowState.motorState[1].dq = lowState.motorState[1].dq_raw = -motor_leg4_j1_dot;
|
||||
lowState.motorState[1].ddq = lowState.motorState[1].ddq_raw = 0;
|
||||
lowState.motorState[2].q = lowState.motorState[2].q_raw = -motor_leg4_j2;
|
||||
lowState.motorState[2].dq = lowState.motorState[2].dq_raw = -motor_leg4_j2_dot;
|
||||
lowState.motorState[2].ddq = lowState.motorState[2].ddq_raw = 0;
|
||||
lowState.motorState[3].q = lowState.motorState[3].q_raw = -motor_leg4_j3;
|
||||
lowState.motorState[3].dq = lowState.motorState[3].dq_raw = -motor_leg4_j3_dot;
|
||||
lowState.motorState[3].ddq = lowState.motorState[3].ddq_raw = 0;
|
||||
|
||||
lowState.motorState[4].q = lowState.motorState[4].q_raw = motor_leg1_j0;
|
||||
lowState.motorState[4].dq = lowState.motorState[4].dq_raw = motor_leg1_j0_dot;
|
||||
lowState.motorState[4].ddq = lowState.motorState[4].ddq_raw = 0;
|
||||
lowState.motorState[5].q = lowState.motorState[5].q_raw = -motor_leg1_j1;
|
||||
lowState.motorState[5].dq = lowState.motorState[5].dq_raw = -motor_leg1_j1_dot;
|
||||
lowState.motorState[5].ddq = lowState.motorState[5].ddq_raw = 0;
|
||||
lowState.motorState[6].q = lowState.motorState[6].q_raw = -motor_leg1_j2;
|
||||
lowState.motorState[6].dq = lowState.motorState[6].dq_raw = -motor_leg1_j2_dot;
|
||||
lowState.motorState[6].ddq = lowState.motorState[6].ddq_raw = 0;
|
||||
lowState.motorState[7].q = lowState.motorState[7].q_raw = -motor_leg1_j3;
|
||||
lowState.motorState[7].dq = lowState.motorState[7].dq_raw = -motor_leg1_j3_dot;
|
||||
lowState.motorState[7].ddq = lowState.motorState[7].ddq_raw = 0;
|
||||
|
||||
lowState.motorState[8].q = lowState.motorState[8].q_raw = motor_leg3_j0;
|
||||
lowState.motorState[8].dq = lowState.motorState[8].dq_raw = motor_leg3_j0_dot;
|
||||
lowState.motorState[8].ddq = lowState.motorState[8].ddq_raw = 0;
|
||||
lowState.motorState[9].q = lowState.motorState[9].q_raw = -motor_leg3_j1;
|
||||
lowState.motorState[9].dq = lowState.motorState[9].dq_raw = -motor_leg3_j1_dot;
|
||||
lowState.motorState[9].ddq = lowState.motorState[9].ddq_raw = 0;
|
||||
lowState.motorState[10].q = lowState.motorState[10].q_raw = -motor_leg3_j2;
|
||||
lowState.motorState[10].dq = lowState.motorState[10].dq_raw = -motor_leg3_j2_dot;
|
||||
lowState.motorState[10].ddq = lowState.motorState[10].ddq_raw = 0;
|
||||
lowState.motorState[11].q = lowState.motorState[11].q_raw = -motor_leg3_j3;
|
||||
lowState.motorState[11].dq = lowState.motorState[11].dq_raw = -motor_leg3_j3_dot;
|
||||
lowState.motorState[11].ddq = lowState.motorState[11].ddq_raw = 0;
|
||||
|
||||
lowState.motorState[12].q = lowState.motorState[12].q_raw = motor_leg2_j0;
|
||||
lowState.motorState[12].dq = lowState.motorState[12].dq_raw = motor_leg2_j0_dot;
|
||||
lowState.motorState[12].ddq = lowState.motorState[12].ddq_raw = 0;
|
||||
lowState.motorState[13].q = lowState.motorState[13].q_raw = -motor_leg2_j1;
|
||||
lowState.motorState[13].dq = lowState.motorState[13].dq_raw = -motor_leg2_j1_dot;
|
||||
lowState.motorState[13].ddq = lowState.motorState[13].ddq_raw = 0;
|
||||
lowState.motorState[14].q = lowState.motorState[14].q_raw = -motor_leg2_j2;
|
||||
lowState.motorState[14].dq = lowState.motorState[14].dq_raw = -motor_leg2_j2_dot;
|
||||
lowState.motorState[14].ddq = lowState.motorState[14].ddq_raw = 0;
|
||||
lowState.motorState[15].q = lowState.motorState[15].q_raw = -motor_leg2_j3;
|
||||
lowState.motorState[15].dq = lowState.motorState[15].dq_raw = -motor_leg2_j3_dot;
|
||||
lowState.motorState[15].ddq = lowState.motorState[15].ddq_raw = 0;
|
||||
|
||||
ctr++;
|
||||
if(ctr>=100)
|
||||
{
|
||||
//float r2d = 180.0/M_PI;
|
||||
//std::cout<<"mcu time = "<<tmp_time_from_mcu<<",\tbattery = "<< BatteryVoltage<<std::endl;
|
||||
//std::cout<<"lx = "<<rockerBtn->lx<<",\try = "<<rockerBtn->ry<<",\trx = "<<rockerBtn->rx<<std::endl;
|
||||
//if(show_rc)
|
||||
if(0)
|
||||
{
|
||||
std::cout<< " rc: " << ((int)(rockerBtn->ly * 100) / 100.0) << "\t" << ((int)(rockerBtn->lx * 100) / 100.0);
|
||||
std::cout<< "\t" << ((int)(rockerBtn->ry * 100) / 100.0) << "\t" << ((int)(rockerBtn->rx * 100) / 100.0)<<std::endl;
|
||||
}
|
||||
|
||||
{
|
||||
std::cout<<"yaw pitch roll = "<<robot_yaw_deg<<"\t"<<robot_pitch_deg<<"\t"<<robot_roll_deg<<std::endl;
|
||||
}
|
||||
|
||||
ctr = 0;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // L4W4_SDK_HPP
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue