EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

v23

This commit is contained in:
fan-ziqi 2025-02-27 23:02:17 +08:00
parent 51ca742147
commit b066b9092e
66 changed files with 1735180 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
.github/workflows/update.yml vendored
View File

@ -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

38
README.md
View File

@ -1,5 +1,10 @@
# rl_sar # rl_sar
[![Ubuntu 20.04/22.04](https://img.shields.io/badge/Ubuntu-20.04/22.04-blue.svg?logo=ubuntu)](https://ubuntu.com/)
[![ROS Noetic](https://img.shields.io/badge/ros-noetic-brightgreen.svg?logo=ros)](https://wiki.ros.org/noetic)
[![ROS2 Foxy/Humble](https://img.shields.io/badge/ros2-foxy/humble-brightgreen.svg?logo=ros)](https://wiki.ros.org/foxy)
[![License](https://img.shields.io/badge/license-Apache2.0-yellow.svg?logo=apache)](https://opensource.org/license/apache-2-0)
[中文文档](README_CN.md) [中文文档](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)** **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,15 @@
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" 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: feature:
- Support legged_gym based on IsaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish. - Built-in pre-trained models for multiple robot simulations, including `a1`, `go2`, `go2w`, `b2`, `b2w`, `gr1t1`, `gr1t2`, `l4w4`;
- The code has two versions: **ROS-Noetic** and **ROS2-Foxy/Humble** - The training framework supports **IsaacGym** and **IsaacSim**, distinguished by `framework`;
- The code supports both cpp and python, you can find python version in `src/rl_sar/scripts` - 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] > [!NOTE]
> If you want to train policy using IsaacLab(IsaacSim), please use [robot_lab](https://github.com/fan-ziqi/robot_lab) project. > 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) > Discuss in [Github Discussion](https://github.com/fan-ziqi/rl_sar/discussions) or [Discord](https://discord.gg/vmVjkhVugU).
## Preparation ## Preparation
@ -56,7 +62,7 @@ sudo apt install libtbb-dev
<details> <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 Install yaml-cpp
@ -92,9 +98,9 @@ catkin build
## Running ## 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 ### Simulation
@ -102,7 +108,7 @@ Open a terminal, launch the gazebo simulation environment
```bash ```bash
source devel/setup.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 Open a new terminal, launch the control program
@ -129,7 +135,9 @@ Gamepad Controls
### Real Robots ### Real Robots
#### Unitree A1 <details>
<summary>Unitree A1 (Click to expand)</summary>
Unitree A1 can be connected using both wireless and wired methods: Unitree A1 can be connected using both wireless and wired methods:
@ -147,7 +155,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. 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). 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. 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 +170,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. 4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1.
</details>
### Train the actuator network ### Train the actuator network
Take A1 as an example below Take A1 as an example below
@ -175,10 +189,10 @@ Take A1 as an example below
## Add Your Robot ## 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. 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. 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. 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. 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.

40
README_CN.md
View File

@ -1,5 +1,10 @@
# rl_sar # rl_sar
[![Ubuntu 20.04/22.04](https://img.shields.io/badge/Ubuntu-20.04/22.04-blue.svg?logo=ubuntu)](https://ubuntu.com/)
[![ROS Noetic](https://img.shields.io/badge/ros-noetic-brightgreen.svg?logo=ros)](https://wiki.ros.org/noetic)
[![ROS2 Foxy/Humble](https://img.shields.io/badge/ros2-foxy/humble-brightgreen.svg?logo=ros)](https://wiki.ros.org/foxy)
[![License](https://img.shields.io/badge/license-Apache2.0-yellow.svg?logo=apache)](https://opensource.org/license/apache-2-0)
[English document](README.md) [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)** **版本选择: [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,15 @@
本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real" 本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real"
特性： 特性：
- 支持基于IsaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，用`framework`加以区分。 - 内置多种机器人仿真的预训练模型，包括 `a1`、`go2`、`go2w`、`b2`、`b2w`、`gr1t1`、`gr1t2`、`l4w4`；
- 代码有**ROS-Noetic**和**ROS2-Foxy/Humble**两个版本 - 训练框架支持**IsaacGym**和**IsaacSim**，用`framework`加以区分；
- 代码有python和cpp两个版本，python版本可以在`src/rl_sar/scripts`中找到 - 代码有**ROS-Noetic**和**ROS2-Foxy/Humble**两个版本；
- 代码有**python**和**cpp**两个版本，其中python版本在`src/rl_sar/scripts`内；
> [!NOTE] > [!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) > 在 [Github Discussion](https://github.com/fan-ziqi/rl_sar/discussions) 或 [Discord](https://discord.gg/MC9KguQHtt) 中讨论
## 准备 ## 准备
@ -56,7 +62,7 @@ sudo apt install libtbb-dev
<details> <details>
<summary>也可以使用源码安装，点击展开</summary> <summary>也可使用源码安装（点击展开）</summary>
安装yaml-cpp 安装yaml-cpp
@ -93,9 +99,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 +109,7 @@ catkin build
```bash ```bash
source devel/setup.bash source devel/setup.bash
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch roslaunch rl_sar gazebo_<ROBOT>.launch cfg:=<CONFIG>
``` ```
打开一个新终端，启动控制程序 打开一个新终端，启动控制程序
@ -130,7 +136,9 @@ source devel/setup.bash
### 真实机器人 ### 真实机器人
#### Unitree A1 <details>
<summary>Unitree A1（点击展开）</summary>
与Unitree A1连接可以使用无线与有线两种方式 与Unitree A1连接可以使用无线与有线两种方式
@ -148,7 +156,11 @@ rosrun rl_sar rl_real_a1
或者按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。 或者按下键盘上的**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”可以改成其他)。 1. 用网线的一端连接Go2机器人，另一端连接用户电脑，并开启电脑的 USB Ethernet 后进行配置。机器狗机载电脑的 IP 地地址为 192.168.123.161，故需将电脑 USB Ethernet 地址设置为与机器狗同一网段，如在 Address 中输入 192.168.123.222 (“222”可以改成其他)。
2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替 2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替
@ -159,6 +171,8 @@ rosrun rl_sar rl_real_a1
``` ```
4. Go2支持手柄与键盘控制，方法与上面a1相同 4. Go2支持手柄与键盘控制，方法与上面a1相同
</details>
### 训练执行器网络 ### 训练执行器网络
下面拿A1举例 下面拿A1举例
@ -176,10 +190,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`的关节配置文件 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()`函数，以适配不同的模型 3. 按需修改代码中的`forward()`函数，以适配不同的模型
4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改

33
src/rl_sar/CMakeLists.txt
View File

@ -43,28 +43,28 @@ link_directories(/usr/local/lib)
include_directories(${YAML_CPP_INCLUDE_DIR}) include_directories(${YAML_CPP_INCLUDE_DIR})
# Unitree A1 # Unitree A1
include_directories(library/unitree_legged_sdk_3.2/include) include_directories(library/thirdparty/unitree_legged_sdk_3.2/include)
link_directories(library/unitree_legged_sdk_3.2/lib) link_directories(library/thirdparty/unitree_legged_sdk_3.2/lib)
set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm) set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)
# Unitree Go2 # Unitree Go2
include_directories(library/unitree_sdk2/include) include_directories(library/thirdparty/unitree_sdk2/include)
link_directories(library/unitree_sdk2/lib/x86_64) link_directories(library/thirdparty/unitree_sdk2/lib/x86_64)
include_directories(library/unitree_sdk2/thirdparty/include) include_directories(library/thirdparty/unitree_sdk2/thirdparty/include)
include_directories(library/unitree_sdk2/thirdparty/include/ddscxx) include_directories(library/thirdparty/unitree_sdk2/thirdparty/include/ddscxx)
link_directories(library/unitree_sdk2/thirdparty/lib/x86_64) link_directories(library/thirdparty/unitree_sdk2/thirdparty/lib/x86_64)
set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx) set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)
include_directories( include_directories(
include include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
library/matplotlibcpp library/core/matplotlibcpp
library/observation_buffer library/core/observation_buffer
library/rl_sdk library/core/rl_sdk
library/loop 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) target_link_libraries(rl_sdk "${TORCH_LIBRARIES}" Python3::Python Python3::Module TBB::tbb)
set_property(TARGET rl_sdk PROPERTY CXX_STANDARD 14) set_property(TARGET rl_sdk PROPERTY CXX_STANDARD 14)
find_package(Python3 COMPONENTS NumPy) find_package(Python3 COMPONENTS NumPy)
@ -74,7 +74,7 @@ else()
target_compile_definitions(rl_sdk WITHOUT_NUMPY) target_compile_definitions(rl_sdk WITHOUT_NUMPY)
endif() 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}") target_link_libraries(observation_buffer "${TORCH_LIBRARIES}")
set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14) 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 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 catkin_install_python(PROGRAMS
scripts/rl_sim.py scripts/rl_sim.py
scripts/actuator_net.py scripts/actuator_net.py

2
src/rl_sar/include/rl_real_a1.hpp
View File

@ -61,8 +61,6 @@ private:
int motiontime = 0; int motiontime = 0;
std::vector<double> mapped_joint_positions; std::vector<double> mapped_joint_positions;
std::vector<double> mapped_joint_velocities; 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_HPP

2
src/rl_sar/include/rl_real_go2.hpp
View File

@ -107,8 +107,6 @@ private:
int motiontime = 0; int motiontime = 0;
std::vector<double> mapped_joint_positions; std::vector<double> mapped_joint_positions;
std::vector<double> mapped_joint_velocities; 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_HPP

60
src/rl_sar/include/rl_real_l4w4.hpp Normal file
View File

@ -0,0 +1,60 @@
/*
* Copyright (c) 2024-2025 Ziqi Fan
* SPDX-License-Identifier: Apache-2.0
*/
#ifndef RL_REAL_HPP
#define RL_REAL_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_HPP

4
src/rl_sar/launch/gazebo_a1_isaacgym.launch → src/rl_sar/launch/gazebo_a1.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="a1"/> <arg name="rname" default="a1"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/> <arg name="cfg" default="isaacgym"/>
<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="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" 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="robot_path" value="(find $(arg rname)_description)"/>

8
src/rl_sar/launch/gazebo_a1_isaacsim.launch → src/rl_sar/launch/gazebo_b2.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="a1"/> <arg name="rname" default="b2"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacsim"/> <arg name="cfg" default="isaacgym"/>
<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="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" 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="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 --> <!-- 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 --> <!-- Set trunk and joint positions at startup -->
<node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen" <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 --> <!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/> <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>

57
src/rl_sar/launch/gazebo_b2w.launch Normal file
View File

@ -0,0 +1,57 @@
<launch>
<arg name="wname" default="stairs"/>
<arg name="rname" default="b2w"/>
<arg name="cfg" default="isaacgym"/>
<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>

4
src/rl_sar/launch/gazebo_go2_isaacgym.launch → src/rl_sar/launch/gazebo_go2.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="go2"/> <arg name="rname" default="go2"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/> <arg name="cfg" default="himloco"/>
<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="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" 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="robot_path" value="(find $(arg rname)_description)"/>

57
src/rl_sar/launch/gazebo_go2w.launch Normal file
View File

@ -0,0 +1,57 @@
<launch>
<arg name="wname" default="stairs"/>
<arg name="rname" default="go2w"/>
<arg name="cfg" default="isaacgym"/>
<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>

4
src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch → src/rl_sar/launch/gazebo_gr1t1.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="gr1t1"/> <arg name="rname" default="gr1t1"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/> <arg name="cfg" default="isaacgym"/>
<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="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" 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="robot_path" value="(find $(arg rname)_description)"/>

4
src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch → src/rl_sar/launch/gazebo_gr1t2.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="gr1t2"/> <arg name="rname" default="gr1t2"/>
<param name="robot_name" type="str" value="$(arg rname)_isaacgym"/> <arg name="cfg" default="isaacgym"/>
<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="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<param name="gazebo_model_name" 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="robot_path" value="(find $(arg rname)_description)"/>

59
src/rl_sar/launch/gazebo_l4w4.launch Normal file
View File

@ -0,0 +1,59 @@
<launch>
<arg name="wname" default="stairs"/>
<arg name="rname" default="l4w4"/>
<arg name="cfg" default="isaacgym"/>
<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"
command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- 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>

48
src/rl_sar/models/a1_isaacgym/config.yaml → src/rl_sar/models/a1/legged_gym/config.yaml
View File

@ -1,41 +1,39 @@
# Copyright (c) 2024-2025 Ziqi Fan # Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0 # SPDX-License-Identifier: Apache-2.0
a1_isaacgym: a1/legged_gym:
model_name: "model_0702.pt" model_name: "model.pt"
framework: "isaacgym" framework: "isaacgym"
rows: 4
cols: 3
dt: 0.005 dt: 0.005
decimation: 4 decimation: 4
num_observations: 45 num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"] observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: [0, 1, 2, 3, 4, 5] observations_history: [0, 1, 2, 3, 4, 5]
clip_obs: 100.0 clip_obs: 100.0
clip_actions_lower: [-100, -100, -100, clip_actions_lower: [-100.0, -100.0, -100.0,
-100, -100, -100, -100.0, -100.0, -100.0,
-100, -100, -100, -100.0, -100.0, -100.0,
-100, -100, -100] -100.0, -100.0, -100.0]
clip_actions_upper: [100, 100, 100, clip_actions_upper: [100.0, 100.0, 100.0,
100, 100, 100, 100.0, 100.0, 100.0,
100, 100, 100, 100.0, 100.0, 100.0,
100, 100, 100] 100.0, 100.0, 100.0]
rl_kp: [20, 20, 20, rl_kp: [20.0, 20.0, 20.0,
20, 20, 20, 20.0, 20.0, 20.0,
20, 20, 20, 20.0, 20.0, 20.0,
20, 20, 20] 20.0, 20.0, 20.0]
rl_kd: [0.5, 0.5, 0.5, rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
0.5, 0.5, 0.5, 0.5, 0.5, 0.5,
0.5, 0.5, 0.5] 0.5, 0.5, 0.5]
fixed_kp: [80, 80, 80, fixed_kp: [80.0, 80.0, 80.0,
80, 80, 80, 80.0, 80.0, 80.0,
80, 80, 80, 80.0, 80.0, 80.0,
80, 80, 80] 80.0, 80.0, 80.0]
fixed_kd: [3, 3, 3, fixed_kd: [3.0, 3.0, 3.0,
3, 3, 3, 3.0, 3.0, 3.0,
3, 3, 3, 3.0, 3.0, 3.0,
3, 3, 3] 3.0, 3.0, 3.0]
hip_scale_reduction: 0.5 hip_scale_reduction: 0.5
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12 num_of_dofs: 12
@ -59,3 +57,5 @@ a1_isaacgym:
"FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"]
command_mapping: [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]
state_mapping: [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]

0
src/rl_sar/models/a1_isaacgym/model_0702.pt → src/rl_sar/models/a1/legged_gym/model.pt
View File

61
src/rl_sar/models/a1/robot_lab/config.yaml Normal file
View File

@ -0,0 +1,61 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
a1/robot_lab:
model_name: "policy.pt"
framework: "isaacsim"
dt: 0.005
decimation: 4
num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0]
rl_kp: [20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0]
rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5]
fixed_kp: [80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0]
fixed_kd: [3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 12
action_scale: 0.25
action_scale_wheel: 0.0
wheel_indices: []
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
default_dof_pos: [ 0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50]
joint_controller_names: ["FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]

BIN
src/rl_sar/models/a1_isaacgym/model_0501.pt
View File

Binary file not shown.

BIN
src/rl_sar/models/a1_isaacgym/model_0522.pt
View File

Binary file not shown.

BIN
src/rl_sar/models/a1_isaacgym/model_0526.pt
View File

Binary file not shown.

61
src/rl_sar/models/a1_isaacsim/config.yaml
View File

@ -1,61 +0,0 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
a1_isaacsim:
model_name: "policy.pt"
framework: "isaacsim"
rows: 4
cols: 3
dt: 0.005
decimation: 4
num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100, -100, -100,
-100, -100, -100,
-100, -100, -100,
-100, -100, -100]
clip_actions_upper: [100, 100, 100,
100, 100, 100,
100, 100, 100,
100, 100, 100]
rl_kp: [20, 20, 20,
20, 20, 20,
20, 20, 20,
20, 20, 20]
rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5]
fixed_kp: [80, 80, 80,
80, 80, 80,
80, 80, 80,
80, 80, 80]
fixed_kd: [3, 3, 3,
3, 3, 3,
3, 3, 3,
3, 3, 3]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 12
action_scale: 0.25
action_scale_wheel: 0.0
wheel_indices: []
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
default_dof_pos: [ 0.1000, 0.8000, -1.5000,
-0.1000, 0.8000, -1.5000,
0.1000, 1.0000, -1.5000,
-0.1000, 1.0000, -1.5000]
joint_controller_names: ["FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"]

BIN
src/rl_sar/models/a1_isaacsim/policy.pt
View File

Binary file not shown.

61
src/rl_sar/models/b2/robot_lab/config.yaml Normal file
View File

@ -0,0 +1,61 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
b2/robot_lab:
model_name: "policy.pt"
framework: "isaacsim"
dt: 0.005
decimation: 4
num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0]
rl_kp: [100.0, 100.0, 150.0,
100.0, 100.0, 150.0,
100.0, 100.0, 150.0,
100.0, 100.0, 150.0]
rl_kd: [1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0]
fixed_kp: [200.0, 200.0, 300.0,
200.0, 200.0, 300.0,
200.0, 200.0, 300.0,
200.0, 200.0, 300.0]
fixed_kd: [3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 12
action_scale: 0.25
action_scale_wheel: 0.0
wheel_indices: []
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [200.0, 200.0, 320.0,
200.0, 200.0, 320.0,
200.0, 200.0, 320.0,
200.0, 200.0, 320.0]
default_dof_pos: [ 0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50]
joint_controller_names: ["FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]

67
src/rl_sar/models/b2w/robot_lab/config.yaml Normal file
View File

@ -0,0 +1,67 @@
b2w/robot_lab:
model_name: "policy.pt"
framework: "isaacsim"
dt: 0.005
decimation: 4
num_observations: 57
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0]
rl_kp: [100.0, 100.0, 150.0,
100.0, 100.0, 150.0,
100.0, 100.0, 150.0,
100.0, 100.0, 150.0,
0.0, 0.0, 0.0, 0.0]
rl_kd: [1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0]
fixed_kp: [200.0, 200.0, 300.0,
200.0, 200.0, 300.0,
200.0, 200.0, 300.0,
200.0, 200.0, 300.0,
0.0, 0.0, 0.0, 0.0]
fixed_kd: [3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
1.0, 1.0, 1.0, 1.0]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 16
action_scale: 0.25
action_scale_wheel: 5.0
wheel_indices: [12, 13, 14, 15]
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [200.0, 200.0, 320.0,
200.0, 200.0, 320.0,
200.0, 200.0, 320.0,
200.0, 200.0, 320.0,
20.0, 20.0, 20.0, 20.0]
default_dof_pos: [ 0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.00, 0.00, 0.00]
joint_controller_names: ["FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"FR_foot_controller", "FL_foot_controller", "RR_foot_controller", "RL_foot_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]

6
src/rl_sar/models/go2_isaacgym/config.yaml → src/rl_sar/models/go2/himloco/config.yaml
View File

@ -1,11 +1,9 @@
# Copyright (c) 2024-2025 Ziqi Fan # Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0 # SPDX-License-Identifier: Apache-2.0
go2_isaacgym: go2/himloco:
model_name: "himloco.pt" model_name: "himloco.pt"
framework: "isaacgym" framework: "isaacgym"
rows: 4
cols: 3
dt: 0.005 dt: 0.005
decimation: 4 decimation: 4
num_observations: 45 num_observations: 45
@ -59,3 +57,5 @@ go2_isaacgym:
"FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"] "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"]
command_mapping: [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]
state_mapping: [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]

0
src/rl_sar/models/go2_isaacgym/himloco.pt → src/rl_sar/models/go2/himloco/himloco.pt
View File

61
src/rl_sar/models/go2/robot_lab/config.yaml Normal file
View File

@ -0,0 +1,61 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
go2/robot_lab:
model_name: "policy.pt"
framework: "isaacsim"
dt: 0.005
decimation: 4
num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0]
rl_kp: [20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0]
rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5]
fixed_kp: [80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0]
fixed_kd: [3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 12
action_scale: 0.25
action_scale_wheel: 0.0
wheel_indices: []
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [23.5, 23.5, 23.5,
23.5, 23.5, 23.5,
23.5, 23.5, 23.5,
23.5, 23.5, 23.5]
default_dof_pos: [ 0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50]
joint_controller_names: ["FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]

67
src/rl_sar/models/go2w/robot_lab/config.yaml Normal file
View File

@ -0,0 +1,67 @@
go2w/robot_lab:
model_name: "policy.pt"
framework: "isaacsim"
dt: 0.005
decimation: 4
num_observations: 57
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: []
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0,
-100.0, -100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0]
rl_kp: [20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
20.0, 20.0, 20.0,
0.0, 0.0, 0.0, 0.0]
rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5, 0.5]
fixed_kp: [80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
80.0, 80.0, 80.0,
0.0, 0.0, 0.0, 0.0]
fixed_kd: [3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
3.0, 3.0, 3.0,
0.5, 0.5, 0.5, 0.5]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: []
num_of_dofs: 16
action_scale: 0.25
action_scale_wheel: 5.0
wheel_indices: [12, 13, 14, 15]
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
torque_limits: [23.5, 23.5, 23.5,
23.5, 23.5, 23.5,
23.5, 23.5, 23.5,
23.5, 23.5, 23.5,
23.5, 23.5, 23.5, 23.5]
default_dof_pos: [ 0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.80, -1.50,
0.00, 0.00, 0.00, 0.00]
joint_controller_names: ["FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
"FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
"RR_hip_controller", "RR_thigh_controller", "RR_calf_controller",
"RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
"FR_foot_controller", "FL_foot_controller", "RR_foot_controller", "RL_foot_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]

6
src/rl_sar/models/gr1t2_isaacgym/config.yaml → src/rl_sar/models/gr1t1/legged_gym/config.yaml
View File

@ -1,11 +1,9 @@
# Copyright (c) 2024-2025 Ziqi Fan # Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0 # SPDX-License-Identifier: Apache-2.0
gr1t2_isaacgym: gr1t1/legged_gym:
model_name: "model_4000_jit.pt" model_name: "model_4000_jit.pt"
framework: "isaacgym" framework: "isaacgym"
rows: 2
cols: 5
dt: 0.001 dt: 0.001
decimation: 20 decimation: 20
num_observations: 39 num_observations: 39
@ -41,3 +39,5 @@ gr1t2_isaacgym:
0.0, 0.0, -0.2618, 0.5236, -0.2618] 0.0, 0.0, -0.2618, 0.5236, -0.2618]
joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller",
"r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]

0
src/rl_sar/models/gr1t1_isaacgym/model_4000_jit.pt → src/rl_sar/models/gr1t1/legged_gym/model_4000_jit.pt
View File

6
src/rl_sar/models/gr1t1_isaacgym/config.yaml → src/rl_sar/models/gr1t2/legged_gym/config.yaml
View File

@ -1,11 +1,9 @@
# Copyright (c) 2024-2025 Ziqi Fan # Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0 # SPDX-License-Identifier: Apache-2.0
gr1t1_isaacgym: gr1t2/legged_gym:
model_name: "model_4000_jit.pt" model_name: "model_4000_jit.pt"
framework: "isaacgym" framework: "isaacgym"
rows: 2
cols: 5
dt: 0.001 dt: 0.001
decimation: 20 decimation: 20
num_observations: 39 num_observations: 39
@ -41,3 +39,5 @@ gr1t1_isaacgym:
0.0, 0.0, -0.2618, 0.5236, -0.2618] 0.0, 0.0, -0.2618, 0.5236, -0.2618]
joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller", joint_controller_names: ["l_hip_roll_controller", "l_hip_yaw_controller", "l_hip_pitch_controller", "l_knee_pitch_controller", "l_ankle_pitch_controller",
"r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"] "r_hip_roll_controller", "r_hip_yaw_controller", "r_hip_pitch_controller", "r_knee_pitch_controller", "r_ankle_pitch_controller"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]

0
src/rl_sar/models/gr1t2_isaacgym/model_4000_jit.pt → src/rl_sar/models/gr1t2/legged_gym/model_4000_jit.pt
View File

63
src/rl_sar/models/l4w4/legged_gym/config.yaml Normal file
View File

@ -0,0 +1,63 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
l4w4/legged_gym:
model_name: "policy_20000.pt"
framework: "isaacgym"
rows: 4
cols: 3
dt: 0.005
decimation: 4
num_observations: 57
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
observations_history: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
clip_obs: 100.0
clip_actions_lower: [-100.0, -100.0, -100.0, -100.0,
-100.0, -100.0, -100.0, -100.0,
-100.0, -100.0, -100.0, -100.0,
-100.0, -100.0, -100.0, -100.0]
clip_actions_upper: [100.0, 100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0,
100.0, 100.0, 100.0, 100.0]
rl_kp: [160.0, 160.0, 160.0, 0.0,
160.0, 160.0, 160.0, 0.0,
160.0, 160.0, 160.0, 0.0,
160.0, 160.0, 160.0, 0.0]
rl_kd: [5.0, 5.0, 5.0, 1.0,
5.0, 5.0, 5.0, 1.0,
5.0, 5.0, 5.0, 1.0,
5.0, 5.0, 5.0, 1.0]
fixed_kp: [180.0, 180.0, 180.0, 0.0,
180.0, 180.0, 180.0, 0.0,
180.0, 180.0, 180.0, 0.0,
180.0, 180.0, 180.0, 0.0]
fixed_kd: [3.0, 3.0, 3.0, 0.5,
3.0, 3.0, 3.0, 0.5,
3.0, 3.0, 3.0, 0.5,
3.0, 3.0, 3.0, 0.5]
hip_scale_reduction: 0.5
hip_scale_reduction_indices: [0, 4, 8, 12]
num_of_dofs: 16
action_scale: 0.5
action_scale_wheel: 8.0
wheel_indices: [3, 7, 11, 15]
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [150.0, 150.0, 150.0, 30.0,
150.0, 150.0, 150.0, 30.0,
150.0, 150.0, 150.0, 30.0,
150.0, 150.0, 150.0, 30.0]
default_dof_pos: [ 0.042, 0.8752, -1.5184, 0.0,
-0.042, 0.8752, -1.5184, 0.0,
0.042, 0.9576, -1.4933, 0.0,
-0.042, 0.9576, -1.4933, 0.0]
joint_controller_names: ["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"]
command_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
state_mapping: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]

BIN
src/rl_sar/models/l4w4/legged_gym/policy_20000.pt Normal file
View File

Binary file not shown.

43
src/rl_sar/scripts/rl_sdk.py
View File

@ -93,6 +93,8 @@ class ModelParams:
self.commands_scale = None self.commands_scale = None
self.default_dof_pos = None self.default_dof_pos = None
self.joint_controller_names = None self.joint_controller_names = None
self.command_mapping = None
self.state_mapping = None
class Observations: class Observations:
def __init__(self): def __init__(self):
@ -363,32 +365,19 @@ class RL:
except AttributeError: except AttributeError:
pass pass
def ReadVectorFromYaml(self, values, framework, rows, cols):
if framework == "isaacsim":
transposed_values = [0] * cols * rows
for r in range(rows):
for c in range(cols):
transposed_values[c * rows + r] = values[r * cols + c]
return transposed_values
elif framework == "isaacgym":
return values
else:
raise ValueError(f"Unsupported framework: {framework}")
def ReadYaml(self, robot_name): def ReadYaml(self, robot_path):
# The config file is located at "rl_sar/src/rl_sar/models/<robot_name>/config.yaml" # The config file is located at "rl_sar/src/rl_sar/models/<robot_path>/config.yaml"
config_path = os.path.join(BASE_PATH, "models", robot_name, "config.yaml") config_path = os.path.join(BASE_PATH, "models", robot_path, "config.yaml")
try: try:
with open(config_path, 'r') as f: with open(config_path, 'r') as f:
config = yaml.safe_load(f)[robot_name] config = yaml.safe_load(f)[robot_path]
except FileNotFoundError as e: except FileNotFoundError as e:
print(LOGGER.ERROR + f"The file '{config_path}' does not exist") print(LOGGER.ERROR + f"The file '{config_path}' does not exist")
return return
self.params.model_name = config["model_name"] self.params.model_name = config["model_name"]
self.params.framework = config["framework"] self.params.framework = config["framework"]
rows = config["rows"]
cols = config["cols"]
self.params.dt = config["dt"] self.params.dt = config["dt"]
self.params.decimation = config["decimation"] self.params.decimation = config["decimation"]
self.params.num_observations = config["num_observations"] self.params.num_observations = config["num_observations"]
@ -402,21 +391,23 @@ class RL:
self.params.clip_actions_upper = None self.params.clip_actions_upper = None
self.params.clip_actions_lower = None self.params.clip_actions_lower = None
else: else:
self.params.clip_actions_upper = torch.tensor(self.ReadVectorFromYaml(config["clip_actions_upper"], self.params.framework, rows, cols)).view(1, -1) self.params.clip_actions_upper = torch.tensor(config["clip_actions_upper"]).view(1, -1)
self.params.clip_actions_lower = torch.tensor(self.ReadVectorFromYaml(config["clip_actions_lower"], self.params.framework, rows, cols)).view(1, -1) self.params.clip_actions_lower = torch.tensor(config["clip_actions_lower"]).view(1, -1)
self.params.num_of_dofs = config["num_of_dofs"] self.params.num_of_dofs = config["num_of_dofs"]
self.params.lin_vel_scale = config["lin_vel_scale"] self.params.lin_vel_scale = config["lin_vel_scale"]
self.params.ang_vel_scale = config["ang_vel_scale"] self.params.ang_vel_scale = config["ang_vel_scale"]
self.params.dof_pos_scale = config["dof_pos_scale"] self.params.dof_pos_scale = config["dof_pos_scale"]
self.params.dof_vel_scale = config["dof_vel_scale"] self.params.dof_vel_scale = config["dof_vel_scale"]
self.params.commands_scale = torch.tensor([self.params.lin_vel_scale, self.params.lin_vel_scale, self.params.ang_vel_scale]) self.params.commands_scale = torch.tensor([self.params.lin_vel_scale, self.params.lin_vel_scale, self.params.ang_vel_scale])
self.params.rl_kp = torch.tensor(self.ReadVectorFromYaml(config["rl_kp"], self.params.framework, rows, cols)).view(1, -1) self.params.rl_kp = torch.tensor(config["rl_kp"]).view(1, -1)
self.params.rl_kd = torch.tensor(self.ReadVectorFromYaml(config["rl_kd"], self.params.framework, rows, cols)).view(1, -1) self.params.rl_kd = torch.tensor(config["rl_kd"]).view(1, -1)
self.params.fixed_kp = torch.tensor(self.ReadVectorFromYaml(config["fixed_kp"], self.params.framework, rows, cols)).view(1, -1) self.params.fixed_kp = torch.tensor(config["fixed_kp"]).view(1, -1)
self.params.fixed_kd = torch.tensor(self.ReadVectorFromYaml(config["fixed_kd"], self.params.framework, rows, cols)).view(1, -1) self.params.fixed_kd = torch.tensor(config["fixed_kd"]).view(1, -1)
self.params.torque_limits = torch.tensor(self.ReadVectorFromYaml(config["torque_limits"], self.params.framework, rows, cols)).view(1, -1) self.params.torque_limits = torch.tensor(config["torque_limits"]).view(1, -1)
self.params.default_dof_pos = torch.tensor(self.ReadVectorFromYaml(config["default_dof_pos"], self.params.framework, rows, cols)).view(1, -1) self.params.default_dof_pos = torch.tensor(config["default_dof_pos"]).view(1, -1)
self.params.joint_controller_names = self.ReadVectorFromYaml(config["joint_controller_names"], self.params.framework, rows, cols) self.params.joint_controller_names = config["joint_controller_names"]
self.params.command_mapping = config["command_mapping"]
self.params.state_mapping = config["state_mapping"]
def CSVInit(self, robot_name): def CSVInit(self, robot_name):
self.csv_filename = os.path.join(BASE_PATH, "models", robot_name, 'motor') self.csv_filename = os.path.join(BASE_PATH, "models", robot_name, 'motor')

6
src/rl_sar/scripts/rl_sim.py
View File

@ -34,7 +34,9 @@ class RL_Sim(RL):
# read params from yaml # read params from yaml
self.robot_name = rospy.get_param("robot_name", "") self.robot_name = rospy.get_param("robot_name", "")
self.ReadYaml(self.robot_name) self.config_name = rospy.get_param("config_name", "")
robot_path = self.robot_name + "/" + self.config_name
self.ReadYaml(robot_path)
for i in range(len(self.params.observations)): for i in range(len(self.params.observations)):
if self.params.observations[i] == "ang_vel": if self.params.observations[i] == "ang_vel":
self.params.observations[i] = "ang_vel_world" self.params.observations[i] = "ang_vel_world"
@ -52,7 +54,7 @@ class RL_Sim(RL):
self.running_state = STATE.STATE_RL_RUNNING self.running_state = STATE.STATE_RL_RUNNING
# model # model
model_path = os.path.join(os.path.dirname(__file__), f"../models/{self.robot_name}/{self.params.model_name}") model_path = os.path.join(os.path.dirname(__file__), f"../models/{robot_path}/{self.params.model_name}")
self.model = torch.jit.load(model_path) self.model = torch.jit.load(model_path)
# publisher # publisher

24
src/rl_sar/src/rl_real_a1.cpp
View File

@ -11,8 +11,10 @@
RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL) RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL)
{ {
// read params from yaml // read params from yaml
this->robot_name = "a1_isaacgym"; this->robot_name = "a1";
this->ReadYaml(this->robot_name); this->config_name = "legged_gym";
std::string robot_path = this->robot_name + "/" + this->config_name;
this->ReadYaml(robot_path);
for (std::string &observation : this->params.observations) for (std::string &observation : this->params.observations)
{ {
// In Unitree A1, the coordinate system for angular velocity is in the body coordinate system. // In Unitree A1, the coordinate system for angular velocity is in the body coordinate system.
@ -38,7 +40,7 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
running_state = STATE_WAITING; running_state = STATE_WAITING;
// model // model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + this->robot_name + "/" + this->params.model_name; std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name;
this->model = torch::jit::load(model_path); this->model = torch::jit::load(model_path);
// loop // loop
@ -119,9 +121,9 @@ void RL_Real::GetState(RobotState<double> *state)
} }
for (int i = 0; i < this->params.num_of_dofs; ++i) for (int i = 0; i < this->params.num_of_dofs; ++i)
{ {
state->motor_state.q[i] = this->unitree_low_state.motorState[state_mapping[i]].q; state->motor_state.q[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].q;
state->motor_state.dq[i] = this->unitree_low_state.motorState[state_mapping[i]].dq; state->motor_state.dq[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].dq;
state->motor_state.tau_est[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst; state->motor_state.tau_est[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].tauEst;
} }
} }
@ -130,11 +132,11 @@ void RL_Real::SetCommand(const RobotCommand<double> *command)
for (int i = 0; i < this->params.num_of_dofs; ++i) for (int i = 0; i < this->params.num_of_dofs; ++i)
{ {
this->unitree_low_command.motorCmd[i].mode = 0x0A; this->unitree_low_command.motorCmd[i].mode = 0x0A;
this->unitree_low_command.motorCmd[i].q = command->motor_command.q[command_mapping[i]]; this->unitree_low_command.motorCmd[i].q = command->motor_command.q[this->params.command_mapping[i]];
this->unitree_low_command.motorCmd[i].dq = command->motor_command.dq[command_mapping[i]]; this->unitree_low_command.motorCmd[i].dq = command->motor_command.dq[this->params.command_mapping[i]];
this->unitree_low_command.motorCmd[i].Kp = command->motor_command.kp[command_mapping[i]]; this->unitree_low_command.motorCmd[i].Kp = command->motor_command.kp[this->params.command_mapping[i]];
this->unitree_low_command.motorCmd[i].Kd = command->motor_command.kd[command_mapping[i]]; this->unitree_low_command.motorCmd[i].Kd = command->motor_command.kd[this->params.command_mapping[i]];
this->unitree_low_command.motorCmd[i].tau = command->motor_command.tau[command_mapping[i]]; this->unitree_low_command.motorCmd[i].tau = command->motor_command.tau[this->params.command_mapping[i]];
} }
this->unitree_safe.PowerProtect(this->unitree_low_command, this->unitree_low_state, 6); this->unitree_safe.PowerProtect(this->unitree_low_command, this->unitree_low_state, 6);

24
src/rl_sar/src/rl_real_go2.cpp
View File

@ -11,8 +11,10 @@
RL_Real::RL_Real() RL_Real::RL_Real()
{ {
// read params from yaml // read params from yaml
this->robot_name = "go2_isaacgym"; this->robot_name = "go2";
this->ReadYaml(this->robot_name); this->config_name = "himloco";
std::string robot_path = this->robot_name + "/" + this->config_name;
this->ReadYaml(robot_path);
for (std::string &observation : this->params.observations) for (std::string &observation : this->params.observations)
{ {
// In Unitree Go2, the coordinate system for angular velocity is in the body coordinate system. // In Unitree Go2, the coordinate system for angular velocity is in the body coordinate system.
@ -54,7 +56,7 @@ RL_Real::RL_Real()
running_state = STATE_WAITING; running_state = STATE_WAITING;
// model // model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + this->robot_name + "/" + this->params.model_name; std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name;
this->model = torch::jit::load(model_path); this->model = torch::jit::load(model_path);
// loop // loop
@ -126,9 +128,9 @@ void RL_Real::GetState(RobotState<double> *state)
} }
for (int i = 0; i < this->params.num_of_dofs; ++i) for (int i = 0; i < this->params.num_of_dofs; ++i)
{ {
state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q(); state->motor_state.q[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].q();
state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq(); state->motor_state.dq[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].dq();
state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est(); state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].tau_est();
} }
} }
@ -137,11 +139,11 @@ void RL_Real::SetCommand(const RobotCommand<double> *command)
for (int i = 0; i < this->params.num_of_dofs; ++i) for (int i = 0; i < this->params.num_of_dofs; ++i)
{ {
this->unitree_low_command.motor_cmd()[i].mode() = 0x01; this->unitree_low_command.motor_cmd()[i].mode() = 0x01;
this->unitree_low_command.motor_cmd()[i].q() = command->motor_command.q[command_mapping[i]]; this->unitree_low_command.motor_cmd()[i].q() = command->motor_command.q[this->params.command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].dq() = command->motor_command.dq[command_mapping[i]]; this->unitree_low_command.motor_cmd()[i].dq() = command->motor_command.dq[this->params.command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kp() = command->motor_command.kp[command_mapping[i]]; this->unitree_low_command.motor_cmd()[i].kp() = command->motor_command.kp[this->params.command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kd() = command->motor_command.kd[command_mapping[i]]; this->unitree_low_command.motor_cmd()[i].kd() = command->motor_command.kd[this->params.command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[command_mapping[i]]; this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[this->params.command_mapping[i]];
} }
this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);

290
src/rl_sar/src/rl_real_l4w4.cpp Normal file
View File

@ -0,0 +1,290 @@
/*
* Copyright (c) 2024-2025 Ziqi Fan
* SPDX-License-Identifier: Apache-2.0
*/
#include "rl_real_l4w4.hpp"
// #define PLOT
// #define CSV_LOGGER
RL_Real::RL_Real()
{
// read params from yaml
this->robot_name = "l4w4";
this->config_name = "legged_gym";
std::string robot_path = this->robot_name + "/" + this->config_name;
this->ReadYaml(robot_path);
for (std::string &observation : this->params.observations)
{
// In Unitree A1, the coordinate system for angular velocity is in the body coordinate system.
if (observation == "ang_vel")
{
observation = "ang_vel_body";
}
}
// init robot
this->l4w4_sdk.InitUDP();
this->l4w4_sdk.InitCmdData(this->l4w4_low_command);
// init rl
torch::autograd::GradMode::set_enabled(false);
torch::set_num_threads(4);
if (!this->params.observations_history.empty())
{
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
}
this->InitObservations();
this->InitOutputs();
this->InitControl();
running_state = STATE_WAITING;
// model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
// loop
this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Real::KeyboardInterface, this));
this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Real::RobotControl, this));
this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.dt * this->params.decimation, std::bind(&RL_Real::RunModel, this));
this->loop_keyboard->start();
this->loop_control->start();
this->loop_rl->start();
#ifdef PLOT
this->plot_t = std::vector<int>(this->plot_size, 0);
this->plot_real_joint_pos.resize(this->params.num_of_dofs);
this->plot_target_joint_pos.resize(this->params.num_of_dofs);
for (auto &vector : this->plot_real_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
for (auto &vector : this->plot_target_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
this->loop_plot = std::make_shared<LoopFunc>("loop_plot", 0.002, std::bind(&RL_Real::Plot, this));
this->loop_plot->start();
#endif
#ifdef CSV_LOGGER
this->CSVInit(this->robot_name);
#endif
}
RL_Real::~RL_Real()
{
this->loop_keyboard->shutdown();
this->loop_control->shutdown();
this->loop_rl->shutdown();
#ifdef PLOT
this->loop_plot->shutdown();
#endif
std::cout << LOGGER::INFO << "RL_Real exit" << std::endl;
}
void RL_Real::GetState(RobotState<double> *state)
{
fd_set rfd;
FD_ZERO(&rfd);
FD_SET(this->l4w4_sdk.client_socket, &rfd);
timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 0;
int ret = select(this->l4w4_sdk.client_socket+1, &rfd, NULL, NULL, &timeout);
if(ret > 0)
{
socklen_t addr_len = sizeof(struct sockaddr_in);
this->l4w4_sdk.recv_len = recvfrom(this->l4w4_sdk.client_socket, this->l4w4_sdk.recv_buff, sizeof(this->l4w4_sdk.recv_buff), 0, (sockaddr*)& this->l4w4_sdk.server_addr, & addr_len);
this->l4w4_sdk.ex_send_recv++;
if(this->l4w4_sdk.recv_len < 32)
{
std::cout<<" udp recv_len = "<<this->l4w4_sdk.recv_len<<std::endl;
return;
}
this->l4w4_sdk.AnalyzeUDP(this->l4w4_sdk.recv_buff, this->l4w4_low_state);
memcpy(&this->unitree_joy, this->l4w4_low_state.wirelessRemote, 40);
if ((int)this->unitree_joy.btn.components.R2 == 1)
{
this->control.control_state = STATE_POS_GETUP;
}
else if ((int)this->unitree_joy.btn.components.R1 == 1)
{
this->control.control_state = STATE_RL_INIT;
}
else if ((int)this->unitree_joy.btn.components.L2 == 1)
{
this->control.control_state = STATE_POS_GETDOWN;
}
if (this->params.framework == "isaacgym")
{
state->imu.quaternion[3] = this->l4w4_low_state.imu.quaternion[0]; // w
state->imu.quaternion[0] = this->l4w4_low_state.imu.quaternion[1]; // x
state->imu.quaternion[1] = this->l4w4_low_state.imu.quaternion[2]; // y
state->imu.quaternion[2] = this->l4w4_low_state.imu.quaternion[3]; // z
}
else if (this->params.framework == "isaacsim")
{
state->imu.quaternion[0] = this->l4w4_low_state.imu.quaternion[0]; // w
state->imu.quaternion[1] = this->l4w4_low_state.imu.quaternion[1]; // x
state->imu.quaternion[2] = this->l4w4_low_state.imu.quaternion[2]; // y
state->imu.quaternion[3] = this->l4w4_low_state.imu.quaternion[3]; // z
}
for (int i = 0; i < 3; ++i)
{
state->imu.gyroscope[i] = this->l4w4_low_state.imu.gyroscope[i];
}
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
state->motor_state.q[i] = this->l4w4_low_state.motorState[this->params.state_mapping[i]].q;
state->motor_state.dq[i] = this->l4w4_low_state.motorState[this->params.state_mapping[i]].dq;
state->motor_state.tau_est[i] = this->l4w4_low_state.motorState[this->params.state_mapping[i]].tauEst;
}
}
else
{
sendto(this->l4w4_sdk.client_socket, this->l4w4_sdk.sent_buff, 96, 0, (const sockaddr*)& this->l4w4_sdk.server_addr, sizeof(this->l4w4_sdk.server_addr));
}
}
void RL_Real::SetCommand(const RobotCommand<double> *command)
{
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
this->l4w4_low_command.motorCmd[i].mode = 0x0A;
this->l4w4_low_command.motorCmd[i].q = command->motor_command.q[this->params.command_mapping[i]];
this->l4w4_low_command.motorCmd[i].dq = command->motor_command.dq[this->params.command_mapping[i]];
this->l4w4_low_command.motorCmd[i].Kp = command->motor_command.kp[this->params.command_mapping[i]];
this->l4w4_low_command.motorCmd[i].Kd = command->motor_command.kd[this->params.command_mapping[i]];
this->l4w4_low_command.motorCmd[i].tau = command->motor_command.tau[this->params.command_mapping[i]];
}
this->l4w4_sdk.SendUDP(this->l4w4_low_command);
}
void RL_Real::RobotControl()
{
this->motiontime++;
this->GetState(&this->robot_state);
this->StateController(&this->robot_state, &this->robot_command);
this->SetCommand(&this->robot_command);
}
void RL_Real::RunModel()
{
// this->l4w4_sdk.PrintMCU(this->running_state);
if (this->running_state == STATE_RL_RUNNING)
{
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
this->obs.commands = torch::tensor({{this->unitree_joy.ly * 1.5f, -this->unitree_joy.lx * 1.5f, -this->unitree_joy.rx * 2}});
// this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0);
this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
torch::Tensor clamped_actions = this->Forward();
this->obs.actions = clamped_actions;
for (int i : this->params.hip_scale_reduction_indices)
{
clamped_actions[0][i] *= this->params.hip_scale_reduction;
}
this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau);
if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0)
{
output_dof_pos_queue.push(this->output_dof_pos);
}
if (this->output_dof_vel.defined() && this->output_dof_vel.numel() > 0)
{
output_dof_vel_queue.push(this->output_dof_vel);
}
if (this->output_dof_tau.defined() && this->output_dof_tau.numel() > 0)
{
output_dof_tau_queue.push(this->output_dof_tau);
}
this->TorqueProtect(this->output_dof_tau);
// this->AttitudeProtect(this->robot_state.imu.quaternion, 75.0f, 75.0f);
#ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0);
this->CSVLogger(this->output_dof_tau, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
#endif
}
}
torch::Tensor RL_Real::Forward()
{
torch::autograd::GradMode::set_enabled(false);
torch::Tensor clamped_obs = this->ComputeObservation();
torch::Tensor actions;
if (!this->params.observations_history.empty())
{
this->history_obs_buf.insert(clamped_obs);
this->history_obs = this->history_obs_buf.get_obs_vec(this->params.observations_history);
// actions = this->model.forward({this->history_obs}).toTensor();
actions = this->model.forward({clamped_obs, history_obs.view({1, 10, 57})}).toTensor();
}
else
{
actions = this->model.forward({clamped_obs}).toTensor();
}
if (this->params.clip_actions_upper.numel() != 0 && this->params.clip_actions_lower.numel() != 0)
{
return torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
}
else
{
return actions;
}
}
void RL_Real::Plot()
{
this->plot_t.erase(this->plot_t.begin());
this->plot_t.push_back(this->motiontime);
plt::cla();
plt::clf();
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
this->plot_real_joint_pos[i].erase(this->plot_real_joint_pos[i].begin());
this->plot_target_joint_pos[i].erase(this->plot_target_joint_pos[i].begin());
this->plot_real_joint_pos[i].push_back(this->l4w4_low_state.motorState[i].q);
this->plot_target_joint_pos[i].push_back(this->l4w4_low_command.motorCmd[i].q);
plt::subplot(4, 3, i + 1);
plt::named_plot("_real_joint_pos", this->plot_t, this->plot_real_joint_pos[i], "r");
plt::named_plot("_target_joint_pos", this->plot_t, this->plot_target_joint_pos[i], "b");
plt::xlim(this->plot_t.front(), this->plot_t.back());
}
// plt::legend();
plt::pause(0.0001);
}
void signalHandler(int signum)
{
exit(0);
}
int main(int argc, char **argv)
{
signal(SIGINT, signalHandler);
RL_Real rl_sar;
while (1)
{
sleep(10);
}
return 0;
}

6
src/rl_sar/src/rl_sim.cpp
View File

@ -14,7 +14,9 @@ RL_Sim::RL_Sim()
// read params from yaml // read params from yaml
nh.param<std::string>("robot_name", this->robot_name, ""); nh.param<std::string>("robot_name", this->robot_name, "");
this->ReadYaml(this->robot_name); nh.param<std::string>("config_name", this->config_name, "");
std::string robot_path = this->robot_name + "/" + this->config_name;
this->ReadYaml(robot_path);
for (std::string &observation : this->params.observations) for (std::string &observation : this->params.observations)
{ {
// In Gazebo, the coordinate system for angular velocity is in the world coordinate system. // In Gazebo, the coordinate system for angular velocity is in the world coordinate system.
@ -38,7 +40,7 @@ RL_Sim::RL_Sim()
running_state = STATE_RL_RUNNING; running_state = STATE_RL_RUNNING;
// model // model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + this->robot_name + "/" + this->params.model_name; std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name;
this->model = torch::jit::load(model_path); this->model = torch::jit::load(model_path);
// publisher // publisher

14
src/robots/l4w4_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(l4w4_description)
find_package(catkin REQUIRED)
catkin_package()
find_package(roslaunch)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)

77
src/robots/l4w4_description/config/robot_control.yaml Normal file
View File

@ -0,0 +1,77 @@
# Copyright (c) 2024-2025 Ziqi Fan
# SPDX-License-Identifier: Apache-2.0
l4w4_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FL_hip_joint
FL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint
FL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FL_calf_joint
FL_foot_controller:
type: robot_joint_controller/RobotJointController
joint: FL_foot_joint
# FR Controllers ---------------------------------------
FR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FR_hip_joint
FR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint
FR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FR_calf_joint
FR_foot_controller:
type: robot_joint_controller/RobotJointController
joint: FR_foot_joint
# RL Controllers ---------------------------------------
RL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RL_hip_joint
RL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint
RL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RL_calf_joint
RL_foot_controller:
type: robot_joint_controller/RobotJointController
joint: RL_foot_joint
# RR Controllers ---------------------------------------
RR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RR_hip_joint
RR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint
RR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RR_calf_joint
RR_foot_controller:
type: robot_joint_controller/RobotJointController
joint: RR_foot_joint

265
src/robots/l4w4_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,265 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 420
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
Enabled: true
Global Options:
Background Color: 238; 238; 238
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.0307118892669678
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.0760490670800209
Y: 0.11421932280063629
Z: -0.1576911211013794
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.0047969818115234
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.558584690093994
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 627
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1108
X: 812
Y: 241

20
src/robots/l4w4_description/launch/display.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<arg
name="model" />
<param
name="robot_description"
textfile="$(find assembly_q4w4_sw2urdf)/urdf/assembly_q4w4_sw2urdf.urdf" />
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" />
<node
name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find assembly_q4w4_sw2urdf)/urdf.rviz" />
</launch>

20
src/robots/l4w4_description/launch/gazebo.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file /home/lidar2/rl_sar-main/src/robots/l4w4_description/urdf/l4w4.urdf -urdf -model l4w4"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

23
src/robots/l4w4_description/launch/l4w4_rviz.launch Normal file
View File

@ -0,0 +1,23 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find l4w4_description)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find l4w4_description)/launch/check_joint.rviz"/>
</launch>

BIN
src/robots/l4w4_description/meshes/shank_R.STL Normal file
View File

Binary file not shown.

98042
src/robots/l4w4_description/meshes/shank_R.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/robots/l4w4_description/meshes/shoulder_R.STL Normal file
View File

Binary file not shown.

126066
src/robots/l4w4_description/meshes/shoulder_R.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/robots/l4w4_description/meshes/thigh_R.STL Normal file
View File

Binary file not shown.

251454
src/robots/l4w4_description/meshes/thigh_R.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/robots/l4w4_description/meshes/trunk.STL Normal file
View File

Binary file not shown.

989842
src/robots/l4w4_description/meshes/trunk.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
src/robots/l4w4_description/meshes/wheel_R.STL Normal file
View File

Binary file not shown.

266470
src/robots/l4w4_description/meshes/wheel_R.dae Normal file
View File

File diff suppressed because one or more lines are too long

21
src/robots/l4w4_description/package.xml Normal file
View File

@ -0,0 +1,21 @@
<package format="2">
<name>l4w4_description</name>
<version>1.0.0</version>
<description>
<p>URDF Description package for l4w4_description</p>
<p>This package contains configuration data, 3D models and launch files
for assembly_q4w4_sw2urdf robot</p>
</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
<export>
<architecture_independent />
</export>
</package>

977
src/robots/l4w4_description/urdf/l4w4.urdf Executable file
View File

@ -0,0 +1,977 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="l4w4">
<link
name="trunk">
<inertial>
<origin
xyz="-0.0107 -0.00135 0.005"
rpy="0 0 0" />
<mass
value="18.057" />
<inertia
ixx="0.12015"
ixy="-0.00086"
ixz="-0.00124"
iyy="0.36210"
iyz="-0.00004"
izz="0.40829" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/trunk.STL" />
</geometry>
<material
name="">
<color
rgba="0.25 0.25 0.25 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<box size="0.688 0.18 0.18" />
</geometry>
</collision>
</link>
<link
name="FR_hip">
<inertial>
<origin
xyz="-0.004726 -0.001176 0.000044"
rpy="0 0 0" />
<mass
value="2" />
<inertia
ixx="0.003012"
ixy="0.000018"
ixz="0.000006"
iyy="0.005188"
iyz="-0.000004"
izz="0.003993" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shoulder_R.STL" />
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0" radius="0" />
</geometry>
</collision>
</link>
<joint
name="FR_hip_joint"
type="revolute">
<origin
xyz="0.249 -0.065 0"
rpy="0 0 0" />
<parent
link="trunk" />
<child
link="FR_hip" />
<axis
xyz="1 0 0" />
<limit
lower="-0.785"
upper="0.785"
effort="130"
velocity="11.5" />
</joint>
<link
name="FR_thigh">
<inertial>
<origin
xyz="-0.007013 -0.09103 -0.06380"
rpy="0 0 0" />
<mass
value="3.396" />
<inertia
ixx="0.043751"
ixy="0.000606"
ixz="0.002054"
iyy="0.043937"
iyz="0.005154"
izz="0.006846" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/thigh_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 -0.1145 -0.100"
rpy="0 0 0" />
<geometry>
<box size="0.122 0.044 0.32" />
</geometry>
</collision>
</link>
<joint
name="FR_thigh_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="FR_hip" />
<child
link="FR_thigh" />
<axis
xyz="0 1 0" />
<limit
lower="-1.4"
upper="3.14"
effort="135"
velocity="12.56" />
</joint>
<link
name="FR_calf">
<inertial>
<origin
xyz="-0.001448 0.001637 -0.130288"
rpy="0 0 0" />
<mass
value="0.829" />
<inertia
ixx="0.012593"
ixy="0.000001"
ixz="-0.000510"
iyy="0.012842"
iyz="-0.000137"
izz="0.000514" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shank_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 -0.2"
rpy="0 0 0" />
<geometry>
<box size="0.06 0.03 0.1"/>
</geometry>
</collision>
</link>
<joint
name="FR_calf_joint"
type="revolute">
<origin
xyz="0 -0.1145 -0.3"
rpy="0 0 0" />
<parent
link="FR_thigh" />
<child
link="FR_calf" />
<axis
xyz="0 1 0" />
<limit
lower="-2.7"
upper="-0.17"
effort="135"
velocity="12.56" />
</joint>
<link
name="FR_foot">
<inertial>
<origin
xyz="0 -0.045 0"
rpy="0 0 0" />
<mass
value="1.939" />
<inertia
ixx="0.003934"
ixy="0.0"
ixz="0.000004"
iyy="0.007113"
iyz="-0.000004"
izz="0.00395" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/wheel_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="0.25 0.25 0.25 1" />
</material>
</visual>
<collision>
<origin
xyz="0 -0.045 0"
rpy="1.570796327 0 0" />
<geometry>
<cylinder length="0.038" radius="0.105"/>
</geometry>
</collision>
</link>
<joint
name="FR_foot_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="FR_calf" />
<child
link="FR_foot" />
<axis
xyz="0 1 0" />
<limit
lower="-1000000"
upper="1000000"
effort="32"
velocity="12.0" />
</joint>
<link
name="FL_hip">
<inertial>
<origin
xyz="-0.004726 0.001176 0.000044"
rpy="0 0 0" />
<mass
value="2" />
<inertia
ixx="0.003012"
ixy="-0.000018"
ixz="0.000006"
iyy="0.005188"
iyz="0.000004"
izz="0.003993" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shoulder_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0" radius="0" />
</geometry>
</collision>
</link>
<joint
name="FL_hip_joint"
type="revolute">
<origin
xyz="0.249 0.065 0"
rpy="0 0 0" />
<parent
link="trunk" />
<child
link="FL_hip" />
<axis
xyz="1 0 0" />
<limit
lower="-0.785"
upper="0.785"
effort="130"
velocity="11.5" />
</joint>
<link
name="FL_thigh">
<inertial>
<origin
xyz="-0.007013 0.09103 -0.06380"
rpy="0 0 0" />
<mass
value="3.396" />
<inertia
ixx="0.043751"
ixy="-0.000606"
ixz="0.002054"
iyy="0.043937"
iyz="-0.005154"
izz="0.006846" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/thigh_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0.1145 -0.100"
rpy="0 0 0" />
<geometry>
<box size="0.122 0.044 0.32" />
</geometry>
</collision>
</link>
<joint
name="FL_thigh_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="FL_hip" />
<child
link="FL_thigh" />
<axis
xyz="0 1 0" />
<limit
lower="-1.4"
upper="3.14"
effort="135"
velocity="12.56" />
</joint>
<link
name="FL_calf">
<inertial>
<origin
xyz="-0.001448 -0.001637 -0.130288"
rpy="0 0 0" />
<mass
value="0.829" />
<inertia
ixx="0.012593"
ixy="-0.000001"
ixz="-0.000510"
iyy="0.012842"
iyz="0.000137"
izz="0.000514" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shank_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 -0.2"
rpy="0 0 0" />
<geometry>
<box size="0.06 0.03 0.1"/>
</geometry>
</collision>
</link>
<joint
name="FL_calf_joint"
type="revolute">
<origin
xyz="0 0.1145 -0.3"
rpy="0 0 0" />
<parent
link="FL_thigh" />
<child
link="FL_calf" />
<axis
xyz="0 1 0" />
<limit
lower="-2.7"
upper="-0.17"
effort="135"
velocity="12.56" />
</joint>
<link
name="FL_foot">
<inertial>
<origin
xyz="0 0.045 0"
rpy="0 0 0" />
<mass
value="1.939" />
<inertia
ixx="0.003934"
ixy="0.0"
ixz="0.000004"
iyy="0.007113"
iyz="0.000004"
izz="0.00395" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/wheel_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="0.25 0.25 0.25 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0.045 0"
rpy="1.570796327 0 0" />
<geometry>
<cylinder length="0.038" radius="0.105"/>
</geometry>
</collision>
</link>
<joint
name="FL_foot_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="FL_calf" />
<child
link="FL_foot" />
<axis
xyz="0 1 0" />
<limit
lower="-1000000"
upper="1000000"
effort="32"
velocity="12.0" />
</joint>
<link
name="RR_hip">
<inertial>
<origin
xyz="0.004726 -0.001176 0.000044"
rpy="0 0 0" />
<mass
value="2" />
<inertia
ixx="0.003012"
ixy="-0.000018"
ixz="-0.000006"
iyy="0.005188"
iyz="-0.000004"
izz="0.003993" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shoulder_R.STL" scale="-1 1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0" radius="0" />
</geometry>
</collision>
</link>
<joint
name="RR_hip_joint"
type="revolute">
<origin
xyz="-0.249 -0.065 0"
rpy="0 0 0" />
<parent
link="trunk" />
<child
link="RR_hip" />
<axis
xyz="1 0 0" />
<limit
lower="-0.785"
upper="0.785"
effort="130"
velocity="11.5" />
</joint>
<link
name="RR_thigh">
<inertial>
<origin
xyz="-0.007013 -0.09103 -0.06380"
rpy="0 0 0" />
<mass
value="3.396" />
<inertia
ixx="0.043751"
ixy="0.000606"
ixz="0.002054"
iyy="0.043937"
iyz="0.005154"
izz="0.006846" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/thigh_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 -0.1145 -0.100"
rpy="0 0 0" />
<geometry>
<box size="0.122 0.044 0.32" />
</geometry>
</collision>
</link>
<joint
name="RR_thigh_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="RR_hip" />
<child
link="RR_thigh" />
<axis
xyz="0 1 0" />
<limit
lower="-1.4"
upper="3.14"
effort="135"
velocity="12.56" />
</joint>
<link
name="RR_calf">
<inertial>
<origin
xyz="-0.001448 0.001637 -0.130288"
rpy="0 0 0" />
<mass
value="0.829" />
<inertia
ixx="0.012593"
ixy="0.000001"
ixz="-0.000510"
iyy="0.012842"
iyz="-0.000137"
izz="0.000514" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shank_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 -0.2"
rpy="0 0 0" />
<geometry>
<box size="0.06 0.03 0.1"/>
</geometry>
</collision>
</link>
<joint
name="RR_calf_joint"
type="revolute">
<origin
xyz="0 -0.1145 -0.3"
rpy="0 0 0" />
<parent
link="RR_thigh" />
<child
link="RR_calf" />
<axis
xyz="0 1 0" />
<limit
lower="-2.7"
upper="-0.17"
effort="135"
velocity="12.56" />
</joint>
<link
name="RR_foot">
<inertial>
<origin
xyz="0 -0.045 0"
rpy="0 0 0" />
<mass
value="1.939" />
<inertia
ixx="0.003934"
ixy="0.0"
ixz="0.000004"
iyy="0.007113"
iyz="-0.000004"
izz="0.00395" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/wheel_R.STL" scale = "1 1 1"/>
</geometry>
<material
name="">
<color
rgba="0.25 0.25 0.25 1" />
</material>
</visual>
<collision>
<origin
xyz="0 -0.045 0"
rpy="1.570796327 0 0" />
<geometry>
<cylinder length="0.038" radius="0.105"/>
</geometry>
</collision>
</link>
<joint
name="RR_foot_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="RR_calf" />
<child
link="RR_foot" />
<axis
xyz="0 1 0" />
<limit
lower="-1000000"
upper="1000000"
effort="32"
velocity="12.56" />
</joint>
<link
name="RL_hip">
<inertial>
<origin
xyz="0.004726 0.001176 0.000044"
rpy="0 0 0" />
<mass
value="2" />
<inertia
ixx="0.003012"
ixy="0.000018"
ixz="-0.000006"
iyy="0.005188"
iyz="0.000004"
izz="0.003993" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shoulder_R.STL" scale = "-1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0" radius="0" />
</geometry>
</collision>
</link>
<joint
name="RL_hip_joint"
type="revolute">
<origin
xyz="-0.249 0.065 0"
rpy="0 0 0" />
<parent
link="trunk" />
<child
link="RL_hip" />
<axis
xyz="1 0 0" />
<limit
lower="-0.785"
upper="0.785"
effort="130"
velocity="11.5" />
</joint>
<link
name="RL_thigh">
<inertial>
<origin
xyz="-0.007013 0.09103 -0.06380"
rpy="0 0 0" />
<mass
value="3.396" />
<inertia
ixx="0.043751"
ixy="-0.000606"
ixz="0.002054"
iyy="0.043937"
iyz="-0.005154"
izz="0.006846" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/thigh_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.7765 0.0314 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0.1145 -0.100"
rpy="0 0 0" />
<geometry>
<box size="0.122 0.044 0.32" />
</geometry>
</collision>
</link>
<joint
name="RL_thigh_joint"
type="revolute">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="RL_hip" />
<child
link="RL_thigh" />
<axis
xyz="0 1 0" />
<limit
lower="-1.4"
upper="3.14"
effort="135"
velocity="12.56" />
</joint>
<link
name="RL_calf">
<inertial>
<origin
xyz="-0.001448 -0.001637 -0.130288"
rpy="0 0 0" />
<mass
value="0.829" />
<inertia
ixx="0.012593"
ixy="-0.000001"
ixz="-0.000510"
iyy="0.012842"
iyz="0.000137"
izz="0.000514" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/shank_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="1 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 -0.2"
rpy="0 0 0" />
<geometry>
<box size="0.06 0.03 0.1"/>
</geometry>
</collision>
</link>
<joint
name="RL_calf_joint"
type="revolute">
<origin
xyz="0 0.1145 -0.3"
rpy="0 0 0" />
<parent
link="RL_thigh" />
<child
link="RL_calf" />
<axis
xyz="0 1 0" />
<limit
lower="-2.7"
upper="-0.17"
effort="135"
velocity="12.56" />
</joint>
<link
name="RL_foot">
<inertial>
<origin
xyz="0 0.045 0"
rpy="0 0 0" />
<mass
value="1.939" />
<inertia
ixx="0.003934"
ixy="0.0"
ixz="0.000004"
iyy="0.007113"
iyz="0.000004"
izz="0.00395" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="../meshes/wheel_R.STL" scale = "1 -1 1"/>
</geometry>
<material
name="">
<color
rgba="0.25 0.25 0.25 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0.045 0"
rpy="1.570796327 0 0" />
<geometry>
<cylinder length="0.038" radius="0.105"/>
</geometry>
</collision>
</link>
<joint
name="RL_foot_joint"
type="revolute">
<origin
xyz="0 0 -0.3"
rpy="0 0 0" />
<parent
link="RL_calf" />
<child
link="RL_foot" />
<axis
xyz="0 1 0" />
<limit
lower="-1000000"
upper="1000000"
effort="32"
velocity="12.0" />
</joint>
</robot>

113
src/robots/l4w4_description/xacro/const.xacro Normal file
View File

@ -0,0 +1,113 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.194"/>
<xacro:property name="trunk_length" value="0.267"/>
<xacro:property name="trunk_height" value="0.114"/>
<xacro:property name="hip_radius" value="0.046"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
<xacro:property name="thigh_shoulder_length" value="0.032"/>
<xacro:property name="thigh_width" value="0.0245"/>
<xacro:property name="thigh_height" value="0.034"/>
<xacro:property name="calf_width" value="0.016"/>
<xacro:property name="calf_height" value="0.016"/>
<xacro:property name="foot_radius" value="0.02"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.0838"/>
<xacro:property name="thigh_length" value="0.2"/>
<xacro:property name="calf_length" value="0.2"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.1805"/>
<xacro:property name="leg_offset_y" value="0.047"/>
<xacro:property name="trunk_offset_z" value="0.01675"/>
<xacro:property name="hip_offset" value="0.065"/>
<!-- joint limits -->
<xacro:property name="damping" value="0"/>
<xacro:property name="friction" value="0"/>
<xacro:property name="hip_max" value="46"/>
<xacro:property name="hip_min" value="-46"/>
<xacro:property name="hip_velocity_max" value="21"/>
<xacro:property name="hip_torque_max" value="33.5"/>
<xacro:property name="thigh_max" value="240"/>
<xacro:property name="thigh_min" value="-60"/>
<xacro:property name="thigh_velocity_max" value="21"/>
<xacro:property name="thigh_torque_max" value="33.5"/>
<xacro:property name="calf_max" value="-52.5"/>
<xacro:property name="calf_min" value="-154.5"/>
<xacro:property name="calf_velocity_max" value="21"/>
<xacro:property name="calf_torque_max" value="33.5"/>
<!-- dynamics inertial value total 13.9kg -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="18.057"/>
<xacro:property name="trunk_com_x" value="-0.0107"/>
<xacro:property name="trunk_com_y" value="-0.00135"/>
<xacro:property name="trunk_com_z" value="0.005"/>
<xacro:property name="trunk_ixx" value="0.12015"/>
<xacro:property name="trunk_ixy" value="-0.00086"/>
<xacro:property name="trunk_ixz" value="-0.00124"/>
<xacro:property name="trunk_iyy" value="0.36210"/>
<xacro:property name="trunk_iyz" value="-0.00004"/>
<xacro:property name="trunk_izz" value="0.40829" />
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="2"/>
<xacro:property name="hip_com_x" value="-0.004726"/>
<xacro:property name="hip_com_y" value="-0.001176"/>
<xacro:property name="hip_com_z" value="0.000044"/>
<xacro:property name="hip_ixx" value="0.003012"/>
<xacro:property name="hip_ixy" value="0.000018"/>
<xacro:property name="hip_ixz" value="0.000006"/>
<xacro:property name="hip_iyy" value="0.005188"/>
<xacro:property name="hip_iyz" value="-0.000004"/>
<xacro:property name="hip_izz" value="0.003993"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="3.396"/>
<xacro:property name="thigh_com_x" value="-0.007013"/>
<xacro:property name="thigh_com_y" value="0.09103"/>
<xacro:property name="thigh_com_z" value="-0.06380"/>
<xacro:property name="thigh_ixx" value="0.043751"/>
<xacro:property name="thigh_ixy" value="-0.000606"/>
<xacro:property name="thigh_ixz" value="0.002054"/>
<xacro:property name="thigh_iyy" value="0.043937"/>
<xacro:property name="thigh_iyz" value="-0.005154"/>
<xacro:property name="thigh_izz" value="0.006846"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.829"/>
<xacro:property name="calf_com_x" value="-0.001448"/>
<xacro:property name="calf_com_y" value="-0.001637"/>
<xacro:property name="calf_com_z" value="-0.130288"/>
<xacro:property name="calf_ixx" value="0.012593"/>
<xacro:property name="calf_ixy" value="-0.000001"/>
<xacro:property name="calf_ixz" value="-0.000510"/>
<xacro:property name="calf_iyy" value="0.012842"/>
<xacro:property name="calf_iyz" value="0.000137"/>
<xacro:property name="calf_izz" value="0.000514"/>
<!-- foot -->
<xacro:property name="foot_mass" value="1.939"/>
<xacro:property name="foot_com_x" value="0"/>
<xacro:property name="foot_com_y" value="0.045"/>
<xacro:property name="foot_com_z" value="0"/>
<xacro:property name="foot_ixx" value="0.003934"/>
<xacro:property name="foot_ixy" value="0.00000"/>
<xacro:property name="foot_ixz" value="0.000004"/>
<xacro:property name="foot_iyy" value="0.007113"/>
<xacro:property name="foot_iyz" value="0.000004"/>
<xacro:property name="foot_izz" value="0.00395"/>
</robot>

266
src/robots/l4w4_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,266 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/l4w4_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Green</material>
</plot>
</plugin>
</gazebo> -->
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo> -->
<!-- Visualization of Foot contacts. -->
<!-- <gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo> -->
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

176
src/robots/l4w4_description/xacro/leg.xacro Normal file
View File

@ -0,0 +1,176 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find l4w4_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
<joint name="${name}_hip_joint" type="revolute">
<xacro:insert_block name="origin"/>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="130" velocity="11.5" lower="-0.785" upper="0.785"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="130" velocity="11.5" lower="-0.785" upper="0.785"/>
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="-0.004726 -0.001176 0.000044"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0.004726 -0.001176 0.000044"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="-0.004726 0.001176 0.000044"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="0.004726 0.001176 0.000044"/>
</xacro:if>
<geometry>
<mesh filename="package://l4w4_description/meshes/shoulder_R.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit lower="-1.4" upper="3.14" effort="135" velocity="12.56"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://l4w4_description/meshes/thigh_R.STL" scale = "1 -1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://l4w4_description/meshes/thigh_R.STL" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.1145 -0.100"/>
<geometry>
<box size="0.122 0.044 0.32"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${0.1145*mirror} -0.3"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit lower="-2.7" upper="-0.17" effort="135" velocity="12.56"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://l4w4_description/meshes/shank_R.STL" scale = "1 -1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://l4w4_description/meshes/shank_R.STL" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<geometry>
<box size="0.06 0.03 0.1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.3"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit lower="-1000000" upper="1000000" effort="32" velocity="12.0"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 -0.0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://l4w4_description/meshes/wheel_R.STL" scale = "1 -1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://l4w4_description/meshes/wheel_R.STL" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.570796327 0 0" xyz="0 -0.045 0"/>
<geometry>
<cylinder length="0.038" radius="0.105"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${foot_com_x} ${foot_com_y} ${foot_com_z}"/>
<mass value="${foot_mass}"/>
<inertia
ixx="${foot_ixx}" ixy="${foot_ixy}" ixz="${foot_ixz}"
iyy="${foot_iyy}" iyz="${foot_iyz}"
izz="${foot_izz}"/>
</inertial>
</link>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

40
src/robots/l4w4_description/xacro/materials.xacro Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

118
src/robots/l4w4_description/xacro/robot.xacro Normal file
View File

@ -0,0 +1,118 @@
<?xml version="1.0"?>
<robot name="l4w4" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:include filename="$(find l4w4_description)/xacro/const.xacro"/>
<xacro:include filename="$(find l4w4_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find l4w4_description)/xacro/stairs.xacro"/>
<xacro:include filename="$(find l4w4_description)/xacro/gazebo.xacro"/>
<xacro:include filename="$(find l4w4_description)/xacro/leg.xacro"/>
<!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/>
<xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rollover Protection mode will add an additional stick on the top, use "true" or "false" to switch it. -->
<xacro:property name="rolloverProtection" value="false"/>
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://l4w4_description/meshes/trunk.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.688 0.18 0.18"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<xacro:if value="${(rolloverProtection == 'true')}">
<joint name="stick_joint" type="fixed">
<parent link="trunk"/>
<child link="stick_link"/>
<origin rpy="0 0 0" xyz="${0.18} 0 ${stick_length/2.0+0.08}"/>
</joint>
<link name="stick_link">
<visual>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="${stick_mass}"/>
<inertia
ixx="${stick_mass / 2.0 * (stick_radius*stick_radius)}" ixy="0.0" ixz="0.0"
iyy="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}" iyz="0.0"
izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>
</inertial>
</link>
</xacro:if>
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>

46
src/robots/l4w4_description/xacro/stairs.xacro Normal file
View File

@ -0,0 +1,46 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="stair_length" value="0.640" />
<xacro:property name="stair_width" value="0.310" />
<xacro:property name="stair_height" value="0.170" />
<xacro:macro name="stairs" params="stairs xpos ypos zpos">
<joint name="stair_joint_origin" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="stair_link_${stairs}"/>
</joint>
<link name="stair_link_${stairs}">
<visual>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
<material name="grey"/>
<origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
</visual>
<collision>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
</collision>
<inertial>
<mass value="0.80"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
</inertial>
</link>
<xacro:if value="${stairs}">
<xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
<joint name="stair_joint_${stairs}" type="fixed">
<parent link="stair_link_${stairs}"/>
<child link="stair_link_${stairs-1}"/>
</joint>
</xacro:if>
</xacro:macro>
</robot>

53
src/robots/l4w4_description/xacro/transmission.xacro Normal file
View File

@ -0,0 +1,53 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_foot_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_foot_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_foot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>