mirror of https://github.com/fan-ziqi/rl_sar.git
fan-ziqi
parent
1a92a2a10c
commit
64fc72d613
|
|
@ -1,6 +1,8 @@
|
|||
build
|
||||
devel
|
||||
logs
|
||||
install
|
||||
log
|
||||
.catkin_tools
|
||||
.vscode
|
||||
*.csv
|
||||
|
|
|
|||
49
README.md
49
README.md
|
|
@ -2,10 +2,13 @@
|
|||
|
||||
[中文文档](README_CN.md)
|
||||
|
||||
**Version Select: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
|
||||
|
||||
This repository provides a framework for simulation verification and physical deployment of robot reinforcement learning algorithms, suitable for quadruped robots, wheeled robots, and humanoid robots. "sar" stands for "simulation and real"
|
||||
|
||||
feature:
|
||||
- Support legged_gym based on IaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish.
|
||||
- The code has two versions: **ROS** and **ROS2**
|
||||
- The code supports both cpp and python, you can find python version in `src/rl_sar/scripts`
|
||||
|
||||
[Click to discuss on Discord](https://discord.gg/vmVjkhVugU)
|
||||
|
|
@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git
|
|||
|
||||
## Dependency
|
||||
|
||||
This project uses `ros-noetic` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages:
|
||||
This project uses `ros2-foxy` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages:
|
||||
|
||||
```bash
|
||||
sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
|
||||
sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro
|
||||
```
|
||||
|
||||
Download and deploy `libtorch` at any location
|
||||
|
|
@ -72,14 +75,17 @@ Compile in the root directory of the project
|
|||
|
||||
```bash
|
||||
cd ..
|
||||
catkin build
|
||||
colcon build --merge-install --symlink-install
|
||||
```
|
||||
|
||||
If catkin build report errors: `Unable to find either executable 'empy' or Python module 'em'`, run `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` before `catkin build`
|
||||
|
||||
## Running
|
||||
|
||||
In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent different environments, which can be `a1_isaacgym`, `a1_isaacsim`, `go2_isaacgym`, `gr1t1_isaacgym`, or `gr1t2_isaacgym`.
|
||||
In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent different environments. Currently supported list:
|
||||
|
||||
| | isaacgym | isaacsim |
|
||||
|-------|----------|----------|
|
||||
| a1 | ✓ | ✓ |
|
||||
| go2 | ✓ | |
|
||||
|
||||
Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`, and configure the parameters in `config.yaml`.
|
||||
|
||||
|
|
@ -88,24 +94,25 @@ Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROB
|
|||
Open a terminal, launch the gazebo simulation environment
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
|
||||
source install/setup.bash
|
||||
ros2 launch rl_sar gazebo.launch.py rname:=<ROBOT> framework:=<PLATFORM>
|
||||
(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym)
|
||||
```
|
||||
|
||||
Open a new terminal, launch the control program
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
(for cpp version) rosrun rl_sar rl_sim
|
||||
(for python version) rosrun rl_sar rl_sim.py
|
||||
source install/setup.bash
|
||||
(for cpp version) ros2 run rl_sar rl_sim
|
||||
(for python version) ros2 run rl_sar rl_sim.py
|
||||
```
|
||||
|
||||
Control:
|
||||
|
||||
* Press **\<Enter\>** to toggle simulation start/stop.
|
||||
<!-- * Press **\<Enter\>** to toggle simulation start/stop. -->
|
||||
* **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis.
|
||||
* Press **\<Space\>** to sets all control commands to zero.
|
||||
* If robot falls down, press **R** to reset Gazebo environment.
|
||||
<!-- * If robot falls down, press **R** to reset Gazebo environment. -->
|
||||
|
||||
### Real Robots
|
||||
|
||||
|
|
@ -119,8 +126,8 @@ Unitree A1 can be connected using both wireless and wired methods:
|
|||
Open a new terminal and start the control program
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun rl_sar rl_real_a1
|
||||
source install/local.bash
|
||||
ros2 run rl_sar rl_real_a1
|
||||
```
|
||||
|
||||
Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right.
|
||||
|
|
@ -133,8 +140,8 @@ Or press **0** on the keyboard to switch the robot to the default standing posit
|
|||
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.
|
||||
3. Open a new terminal and start the control program:
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
|
||||
source install/local.bash
|
||||
ros2 run rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
|
||||
```
|
||||
4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1.
|
||||
|
||||
|
|
@ -146,11 +153,11 @@ Take A1 as an example below
|
|||
2. Run the control program, and the program will log all data after execution.
|
||||
3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths.
|
||||
```bash
|
||||
rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
|
||||
ros2 run rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
|
||||
```
|
||||
4. Verify the trained actuator network.
|
||||
```bash
|
||||
rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
|
||||
ros2 run rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
|
||||
```
|
||||
|
||||
## Add Your Robot
|
||||
|
|
@ -160,8 +167,8 @@ In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent your robo
|
|||
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.
|
||||
3. Modify the `forward()` function in the code as needed to adapt to different models.
|
||||
4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory.
|
||||
5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
|
||||
<!-- 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 on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
|
||||
|
||||
## Contributing
|
||||
|
||||
|
|
|
|||
53
README_CN.md
53
README_CN.md
|
|
@ -2,10 +2,13 @@
|
|||
|
||||
[English document](README.md)
|
||||
|
||||
**版本选择: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
|
||||
|
||||
本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real"
|
||||
|
||||
特性：
|
||||
- 支持基于IaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，用`framework`加以区分。
|
||||
- 代码有**ROS**和**ROS2**两个版本
|
||||
- 代码有python和cpp两个版本，python版本可以在`src/rl_sar/scripts`中找到
|
||||
|
||||
[点击在Discord上讨论](https://discord.gg/MC9KguQHtt)
|
||||
|
|
@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git
|
|||
|
||||
## 依赖
|
||||
|
||||
本项目使用`ros-noetic`(Ubuntu20.04)，且需要安装以下的ros依赖包
|
||||
本项目使用`ros2-foxy`(Ubuntu20.04)，且需要安装以下的ros依赖包
|
||||
|
||||
```bash
|
||||
sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
|
||||
sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro
|
||||
```
|
||||
|
||||
在任意位置下载并部署`libtorch`
|
||||
|
|
@ -72,14 +75,17 @@ sudo ldconfig
|
|||
|
||||
```bash
|
||||
cd ..
|
||||
catkin build
|
||||
colcon build --merge-install --symlink-install
|
||||
```
|
||||
|
||||
如果 catkin build 报错: `Unable to find either executable 'empy' or Python module 'em'`, 在`catkin build` 之前执行 `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3`
|
||||
|
||||
## 运行
|
||||
|
||||
下文中使用 **\<ROBOT\>_\<PLATFORM\>** 代替表示不同的环境，可以是 `a1_isaacgym` 、 `a1_isaacsim` 、 `go2_isaacgym` 、 `gr1t1_isaacgym` 、 `gr1t2_isaacgym`
|
||||
下文中使用 **\<ROBOT\>** 和 **\<PLATFORM\>** 代替表示不同的机器人和框架。目前支持列表：
|
||||
|
||||
| | isaacgym | isaacsim |
|
||||
|-------|----------|----------|
|
||||
| a1 | ✓ | ✓ |
|
||||
| go2 | ✓ | |
|
||||
|
||||
运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`中，并配置`config.yaml`中的参数。
|
||||
|
||||
|
|
@ -88,24 +94,25 @@ catkin build
|
|||
打开一个终端，启动gazebo仿真环境
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
|
||||
source install/setup.bash
|
||||
ros2 launch rl_sar gazebo.launch.py rname:=<ROBOT> framework:=<PLATFORM>
|
||||
(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym)
|
||||
```
|
||||
|
||||
打开一个新终端，启动控制程序
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
(for cpp version) rosrun rl_sar rl_sim
|
||||
(for python version) rosrun rl_sar rl_sim.py
|
||||
source install/setup.bash
|
||||
(for cpp version) ros2 run rl_sar rl_sim
|
||||
(for python version) ros2 run rl_sar rl_sim.py
|
||||
```
|
||||
|
||||
控制：
|
||||
|
||||
* 按 **\<Enter\>** 切换仿真器运行/停止。
|
||||
* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。
|
||||
<!-- * 按 **\<Enter\>** 切换仿真器运行/停止。 -->
|
||||
* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴。
|
||||
* 按 **\<Space\>** 将所有控制指令设置为零。
|
||||
* 如果机器人摔倒，按 **R** 重置Gazebo环境。
|
||||
<!-- * 如果机器人摔倒，按 **R** 重置Gazebo环境。 -->
|
||||
|
||||
### 真实机器人
|
||||
|
||||
|
|
@ -119,8 +126,8 @@ source devel/setup.bash
|
|||
新建终端，启动控制程序
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun rl_sar rl_real_a1
|
||||
source install/local.bash
|
||||
ros2 run rl_sar rl_real_a1
|
||||
```
|
||||
|
||||
按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。
|
||||
|
|
@ -133,8 +140,8 @@ rosrun rl_sar rl_real_a1
|
|||
2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替
|
||||
3. 新建终端，启动控制程序
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
|
||||
source install/local.bash
|
||||
ros2 run rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
|
||||
```
|
||||
4. Go2支持手柄与键盘控制，方法与上面a1相同
|
||||
|
||||
|
|
@ -144,13 +151,13 @@ rosrun rl_sar rl_real_a1
|
|||
|
||||
1. 取消注释`rl_real_a1.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。
|
||||
2. 运行控制程序，程序会在执行后记录所有数据。
|
||||
3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。
|
||||
3. 停止控制程序，开始训练执行器网络。
|
||||
```bash
|
||||
rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
|
||||
ros2 run rl_sar actuator_net.py --mode train --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt
|
||||
```
|
||||
4. 验证已经训练好的训练执行器网络。
|
||||
```bash
|
||||
rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
|
||||
ros2 run rl_sar actuator_net.py --mode play --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt
|
||||
```
|
||||
|
||||
## 添加你的机器人
|
||||
|
|
@ -160,8 +167,8 @@ rosrun rl_sar rl_real_a1
|
|||
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`文件修改其中参数
|
||||
3. 按需修改代码中的`forward()`函数，以适配不同的模型
|
||||
4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
|
||||
5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
|
||||
<!-- 4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 -->
|
||||
4. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
|
||||
|
||||
## 贡献
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
cmake_minimum_required(VERSION 3.0.2)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(rl_sar)
|
||||
|
||||
add_definitions(-DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
|
||||
|
|
@ -10,33 +10,27 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
|
|||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
|
||||
find_package(Torch REQUIRED)
|
||||
set(CMAKE_INSTALL_RPATH "${Torch_DIR}/lib")
|
||||
set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
|
||||
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
|
||||
find_package(gazebo REQUIRED)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
controller_manager
|
||||
genmsg
|
||||
joint_state_controller
|
||||
robot_state_publisher
|
||||
roscpp
|
||||
gazebo_ros
|
||||
std_msgs
|
||||
tf
|
||||
geometry_msgs
|
||||
robot_msgs
|
||||
robot_joint_controller
|
||||
rospy
|
||||
)
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(joint_state_controller REQUIRED)
|
||||
find_package(robot_state_publisher REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(gazebo_ros REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(robot_msgs REQUIRED)
|
||||
find_package(robot_joint_controller REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(gazebo_msgs REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
|
||||
find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
robot_joint_controller
|
||||
rospy
|
||||
)
|
||||
|
||||
link_directories(/usr/local/lib)
|
||||
include_directories(${YAML_CPP_INCLUDE_DIR})
|
||||
|
||||
|
|
@ -44,6 +38,11 @@ include_directories(${YAML_CPP_INCLUDE_DIR})
|
|||
include_directories(library/unitree_legged_sdk_3.2/include)
|
||||
link_directories(library/unitree_legged_sdk_3.2/lib)
|
||||
set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)
|
||||
file(GLOB GLOB_UNITREE_LEGGED_SDK "${PROJECT_SOURCE_DIR}/library/unitree_legged_sdk_3.2/lib/*.so")
|
||||
install(FILES
|
||||
${GLOB_UNITREE_LEGGED_SDK}
|
||||
DESTINATION lib/
|
||||
)
|
||||
|
||||
# Unitree Go2
|
||||
include_directories(library/unitree_sdk2/include)
|
||||
|
|
@ -52,10 +51,16 @@ include_directories(library/unitree_sdk2/thirdparty/include)
|
|||
include_directories(library/unitree_sdk2/thirdparty/include/ddscxx)
|
||||
link_directories(library/unitree_sdk2/thirdparty/lib/x86_64)
|
||||
set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)
|
||||
file(GLOB GLOB_UNITREE_SDK2 "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/lib/x86_64/*.so")
|
||||
file(GLOB GLOB_UNITREE_SDK2_THIRDPARTY "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/thirdparty/lib/x86_64/*.so")
|
||||
install(FILES
|
||||
${GLOB_UNITREE_SDK2_THIRDPARTY}
|
||||
${GLOB_UNITREE_SDK2_THIRDPARTY}
|
||||
DESTINATION lib/
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
library/matplotlibcpp
|
||||
library/observation_buffer
|
||||
library/rl_sdk
|
||||
|
|
@ -78,24 +83,53 @@ set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14)
|
|||
|
||||
add_executable(rl_sim src/rl_sim.cpp )
|
||||
target_link_libraries(rl_sim
|
||||
${catkin_LIBRARIES} -pthread
|
||||
-pthread
|
||||
rl_sdk observation_buffer yaml-cpp
|
||||
)
|
||||
|
||||
add_executable(rl_real_a1 src/rl_real_a1.cpp )
|
||||
target_link_libraries(rl_real_a1
|
||||
${catkin_LIBRARIES} ${UNITREE_A1_LIBS}
|
||||
${UNITREE_A1_LIBS}
|
||||
rl_sdk observation_buffer yaml-cpp
|
||||
)
|
||||
|
||||
add_executable(rl_real_go2 src/rl_real_go2.cpp )
|
||||
target_link_libraries(rl_real_go2
|
||||
${catkin_LIBRARIES} ${UNITREE_GO2_LIBS}
|
||||
${UNITREE_GO2_LIBS}
|
||||
rl_sdk observation_buffer yaml-cpp
|
||||
)
|
||||
|
||||
catkin_install_python(PROGRAMS
|
||||
ament_target_dependencies(rl_sim
|
||||
joint_state_controller
|
||||
robot_state_publisher
|
||||
rclcpp
|
||||
gazebo_ros
|
||||
std_msgs
|
||||
robot_msgs
|
||||
robot_joint_controller
|
||||
rclpy
|
||||
gazebo_msgs
|
||||
std_srvs
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
rl_sim
|
||||
rl_sdk
|
||||
observation_buffer
|
||||
rl_real_a1
|
||||
rl_real_go2
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/rl_sim.py
|
||||
scripts/actuator_net.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch worlds models
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -4,19 +4,22 @@
|
|||
#include "rl_sdk.hpp"
|
||||
#include "observation_buffer.hpp"
|
||||
#include "loop.hpp"
|
||||
#include <ros/ros.h>
|
||||
#include <gazebo_msgs/ModelStates.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
#include "std_srvs/Empty.h"
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "robot_msgs/MotorCommand.h"
|
||||
#include "robot_msgs/msg/robot_command.hpp"
|
||||
#include "robot_msgs/msg/robot_state.hpp"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
#include <gazebo_msgs/srv/set_model_state.hpp>
|
||||
#include <rcl_interfaces/srv/get_parameters.hpp>
|
||||
|
||||
#include <csignal>
|
||||
#include <gazebo_msgs/SetModelState.h>
|
||||
|
||||
#include "matplotlibcpp.h"
|
||||
namespace plt = matplotlibcpp;
|
||||
|
||||
class RL_Sim : public RL
|
||||
class RL_Sim : public RL, public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RL_Sim();
|
||||
|
|
@ -48,29 +51,27 @@ private:
|
|||
|
||||
// ros interface
|
||||
std::string ros_namespace;
|
||||
geometry_msgs::Twist vel;
|
||||
geometry_msgs::Pose pose;
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
ros::Subscriber model_state_subscriber;
|
||||
ros::Subscriber joint_state_subscriber;
|
||||
ros::Subscriber cmd_vel_subscriber;
|
||||
ros::ServiceClient gazebo_set_model_state_client;
|
||||
ros::ServiceClient gazebo_pause_physics_client;
|
||||
ros::ServiceClient gazebo_unpause_physics_client;
|
||||
std::map<std::string, ros::Publisher> joint_publishers;
|
||||
std::vector<robot_msgs::MotorCommand> joint_publishers_commands;
|
||||
void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);
|
||||
void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
|
||||
void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
|
||||
sensor_msgs::msg::Imu gazebo_imu;
|
||||
geometry_msgs::msg::Twist cmd_vel;
|
||||
robot_msgs::msg::RobotCommand robot_command_publisher_msg;
|
||||
robot_msgs::msg::RobotState robot_state_subscriber_msg;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr gazebo_imu_subscriber;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber;
|
||||
rclcpp::Client<gazebo_msgs::srv::SetModelState>::SharedPtr gazebo_set_model_state_client;
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr gazebo_pause_physics_client;
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr gazebo_unpause_physics_client;
|
||||
rclcpp::Publisher<robot_msgs::msg::RobotCommand>::SharedPtr robot_command_publisher;
|
||||
rclcpp::Subscription<robot_msgs::msg::RobotState>::SharedPtr robot_state_subscriber;
|
||||
rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr param_client;
|
||||
void GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg);
|
||||
|
||||
// others
|
||||
std::string gazebo_model_name;
|
||||
int motiontime = 0;
|
||||
std::map<std::string, size_t> sorted_to_original_index;
|
||||
std::vector<double> mapped_joint_positions;
|
||||
std::vector<double> mapped_joint_velocities;
|
||||
std::vector<double> mapped_joint_efforts;
|
||||
void MapData(const std::vector<double> &source_data, std::vector<double> &target_data);
|
||||
};
|
||||
|
||||
#endif // RL_SIM_HPP
|
||||
|
|
|
|||
|
|
@ -0,0 +1,94 @@
|
|||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, TextSubstitution, Command
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
rname = LaunchConfiguration("rname")
|
||||
framework = LaunchConfiguration("framework")
|
||||
|
||||
wname = "stair"
|
||||
robot_name = ParameterValue(Command(["echo -n ", rname, "_", framework]), value_type=str)
|
||||
ros_namespace = ParameterValue(Command(["echo -n ", "/", rname, "_gazebo"]), value_type=str)
|
||||
gazebo_model_name = ParameterValue(Command(["echo -n ", rname, "_gazebo"]), value_type=str)
|
||||
|
||||
robot_description = ParameterValue(
|
||||
Command([
|
||||
"xacro ",
|
||||
Command(["echo -n ", Command(["ros2 pkg prefix ", rname, "_description"])]),
|
||||
"/share/", rname, "_description/xacro/robot.xacro"
|
||||
]),
|
||||
value_type=str
|
||||
)
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description}],
|
||||
)
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"verbose": "false",
|
||||
"world": os.path.join(get_package_share_directory("rl_sar"), "worlds", wname + ".world"),
|
||||
}.items(),
|
||||
)
|
||||
|
||||
spawn_entity = Node(
|
||||
package="gazebo_ros",
|
||||
executable="spawn_entity.py",
|
||||
arguments=["-topic", "/robot_description", "-entity", "robot_model"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
joint_state_broadcaster_node = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["joint_state_broadcaster"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
robot_joint_controller_node = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["robot_joint_controller"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
param_node = Node(
|
||||
package="demo_nodes_cpp",
|
||||
executable="parameter_blackboard",
|
||||
name="param_node",
|
||||
parameters=[{
|
||||
"robot_name": robot_name,
|
||||
"gazebo_model_name": gazebo_model_name,
|
||||
}],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"rname",
|
||||
description="Robot name (e.g., a1, go2)",
|
||||
default_value=TextSubstitution(text=""),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"framework",
|
||||
description="Framework (isaacgym or isaacsim)",
|
||||
default_value=TextSubstitution(text=""),
|
||||
),
|
||||
robot_state_publisher_node,
|
||||
gazebo,
|
||||
spawn_entity,
|
||||
joint_state_broadcaster_node,
|
||||
robot_joint_controller_node,
|
||||
param_node,
|
||||
])
|
||||
|
|
@ -486,6 +486,7 @@ void RL::CSVInit(std::string robot_name)
|
|||
// csv_filename += "_" + timestamp;
|
||||
|
||||
csv_filename += ".csv";
|
||||
std::cout << LOGGER::INFO << "Recording motor data in " << csv_filename << std::endl;
|
||||
std::ofstream file(csv_filename.c_str());
|
||||
|
||||
for(int i = 0; i < 12; ++i) { file << "tau_cal_" << i << ","; }
|
||||
|
|
|
|||
|
|
@ -11,10 +11,10 @@
|
|||
|
||||
namespace LOGGER
|
||||
{
|
||||
const char *const INFO = "\033[0;37m[INFO]\033[0m ";
|
||||
const char *const WARNING = "\033[0;33m[WARNING]\033[0m ";
|
||||
const char *const ERROR = "\033[0;31m[ERROR]\033[0m ";
|
||||
const char *const DEBUG = "\033[0;32m[DEBUG]\033[0m ";
|
||||
const std::string INFO = "\033[0;37m[INFO]\033[0m ";
|
||||
const std::string WARNING = "\033[0;33m[WARNING]\033[0m ";
|
||||
const std::string ERROR = "\033[0;31m[ERROR]\033[0m ";
|
||||
const std::string DEBUG = "\033[0;32m[DEBUG]\033[0m ";
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
|
|
@ -45,7 +45,7 @@ struct RobotState
|
|||
std::vector<T> q = std::vector<T>(32, 0.0);
|
||||
std::vector<T> dq = std::vector<T>(32, 0.0);
|
||||
std::vector<T> ddq = std::vector<T>(32, 0.0);
|
||||
std::vector<T> tauEst = std::vector<T>(32, 0.0);
|
||||
std::vector<T> tau_est = std::vector<T>(32, 0.0);
|
||||
std::vector<T> cur = std::vector<T>(32, 0.0);
|
||||
} motor_state;
|
||||
};
|
||||
|
|
@ -156,7 +156,7 @@ public:
|
|||
// others
|
||||
std::string robot_name;
|
||||
STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING
|
||||
bool simulation_running = false;
|
||||
bool simulation_running = true;
|
||||
|
||||
// protect func
|
||||
void TorqueProtect(torch::Tensor origin_output_torques);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<package format="3">
|
||||
<name>rl_sar</name>
|
||||
<version>2.0.0</version>
|
||||
<description>The rl_sar package</description>
|
||||
|
|
@ -8,29 +8,19 @@
|
|||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<buildtool_depend>genmsg</buildtool_depend>
|
||||
<build_depend>controller_manager</build_depend>
|
||||
<build_depend>joint_state_controller</build_depend>
|
||||
<build_depend>robot_state_publisher</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>controller_manager</build_export_depend>
|
||||
<build_export_depend>joint_state_controller</build_export_depend>
|
||||
<build_export_depend>robot_state_publisher</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>joint_state_controller</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>gazebo_ros</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
<depend>robot_msgs</depend>
|
||||
<depend>robot_joint_controller</depend>
|
||||
<depend>controller_manager</depend>
|
||||
<depend>joint_state_controller</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>rospy</depend>
|
||||
|
||||
<export>
|
||||
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import argparse
|
||||
from matplotlib import pyplot as plt
|
||||
|
|
@ -8,8 +9,9 @@ import torch.nn.functional as F
|
|||
from torch.utils.data import Dataset, DataLoader
|
||||
from torch.optim import Adam
|
||||
import pandas as pd
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
BASE_PATH = os.path.join(os.path.dirname(__file__), "../")
|
||||
BASE_PATH = os.path.join(get_package_share_directory('rl_sar'))
|
||||
|
||||
class Config:
|
||||
def __init__(self):
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@ class RobotState:
|
|||
self.q = [0.0] * 32
|
||||
self.dq = [0.0] * 32
|
||||
self.ddq = [0.0] * 32
|
||||
self.tauEst = [0.0] * 32
|
||||
self.tau_est = [0.0] * 32
|
||||
self.cur = [0.0] * 32
|
||||
|
||||
class STATE(Enum):
|
||||
|
|
@ -123,7 +123,7 @@ class RL:
|
|||
# others
|
||||
self.robot_name = ""
|
||||
self.running_state = STATE.STATE_RL_RUNNING # default running_state set to STATE_RL_RUNNING
|
||||
self.simulation_running = False
|
||||
self.simulation_running = True
|
||||
|
||||
### protected in cpp ###
|
||||
# rl module
|
||||
|
|
@ -228,10 +228,10 @@ class RL:
|
|||
self.getup_percent = min(self.getup_percent, 1.0)
|
||||
for i in range(self.params.num_of_dofs):
|
||||
command.motor_command.q[i] = (1 - self.getup_percent) * self.now_state.motor_state.q[i] + self.getup_percent * self.params.default_dof_pos[0][i].item()
|
||||
command.motor_command.dq[i] = 0
|
||||
command.motor_command.dq[i] = 0.0
|
||||
command.motor_command.kp[i] = self.params.fixed_kp[0][i].item()
|
||||
command.motor_command.kd[i] = self.params.fixed_kd[0][i].item()
|
||||
command.motor_command.tau[i] = 0
|
||||
command.motor_command.tau[i] = 0.0
|
||||
print("\r" + LOGGER.INFO + f"Getting up {self.getup_percent * 100.0:.1f}", end='', flush=True)
|
||||
|
||||
if self.control.control_state == STATE.STATE_RL_INIT:
|
||||
|
|
@ -261,10 +261,10 @@ class RL:
|
|||
print("\r" + LOGGER.INFO + f"RL Controller x: {self.control.x:.1f} y: {self.control.y:.1f} yaw: {self.control.yaw:.1f}", end='', flush=True)
|
||||
for i in range(self.params.num_of_dofs):
|
||||
command.motor_command.q[i] = self.output_dof_pos[0][i].item()
|
||||
command.motor_command.dq[i] = 0
|
||||
command.motor_command.dq[i] = 0.0
|
||||
command.motor_command.kp[i] = self.params.rl_kp[0][i].item()
|
||||
command.motor_command.kd[i] = self.params.rl_kd[0][i].item()
|
||||
command.motor_command.tau[i] = 0
|
||||
command.motor_command.tau[i] = 0.0
|
||||
|
||||
if self.control.control_state == STATE.STATE_POS_GETDOWN:
|
||||
self.control.control_state = STATE.STATE_WAITING
|
||||
|
|
@ -289,10 +289,10 @@ class RL:
|
|||
self.getdown_percent = min(1.0, self.getdown_percent)
|
||||
for i in range(self.params.num_of_dofs):
|
||||
command.motor_command.q[i] = (1 - self.getdown_percent) * self.now_state.motor_state.q[i] + self.getdown_percent * self.start_state.motor_state.q[i]
|
||||
command.motor_command.dq[i] = 0
|
||||
command.motor_command.dq[i] = 0.0
|
||||
command.motor_command.kp[i] = self.params.fixed_kp[0][i].item()
|
||||
command.motor_command.kd[i] = self.params.fixed_kd[0][i].item()
|
||||
command.motor_command.tau[i] = 0
|
||||
command.motor_command.tau[i] = 0.0
|
||||
print("\r" + LOGGER.INFO + f"Getting down {self.getdown_percent * 100.0:.1f}", end='', flush=True)
|
||||
|
||||
if self.getdown_percent == 1:
|
||||
|
|
@ -424,6 +424,7 @@ class RL:
|
|||
# self.csv_filename += f"_{timestamp}"
|
||||
|
||||
self.csv_filename += ".csv"
|
||||
print(LOGGER.INFO + f"Recording motor data in '{os.path.abspath(self.csv_filename)}'")
|
||||
|
||||
with open(self.csv_filename, 'w', newline='') as file:
|
||||
writer = csv.writer(file)
|
||||
|
|
|
|||
|
|
@ -1,90 +1,94 @@
|
|||
import sys
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import torch
|
||||
import threading
|
||||
import time
|
||||
import rospy
|
||||
import numpy as np
|
||||
from gazebo_msgs.msg import ModelStates
|
||||
from sensor_msgs.msg import JointState
|
||||
from geometry_msgs.msg import Twist, Pose
|
||||
from robot_msgs.msg import MotorCommand
|
||||
from gazebo_msgs.srv import SetModelState, SetModelStateRequest
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_system_default
|
||||
from sensor_msgs.msg import Imu
|
||||
from geometry_msgs.msg import Twist
|
||||
from robot_msgs.msg import RobotCommand as RobotCommandMsg
|
||||
from robot_msgs.msg import RobotState as RobotStateMsg
|
||||
from robot_msgs.msg import MotorCommand as MotorCommandMsg
|
||||
from robot_msgs.msg import MotorState as MotorStateMsg
|
||||
from gazebo_msgs.srv import SetModelState
|
||||
from std_srvs.srv import Empty
|
||||
|
||||
path = os.path.abspath(".")
|
||||
sys.path.insert(0, path + "/src/rl_sar/scripts")
|
||||
from rcl_interfaces.srv import GetParameters
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from rl_sdk import *
|
||||
from observation_buffer import *
|
||||
|
||||
CSV_LOGGER = False
|
||||
CSV_LOGGER = True
|
||||
|
||||
class RL_Sim(RL):
|
||||
class RL_Sim(RL, Node):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
Node.__init__(self, "rl_sim_node")
|
||||
RL.__init__(self)
|
||||
|
||||
# member variables for RL_Sim
|
||||
self.vel = Twist()
|
||||
self.pose = Pose()
|
||||
self.cmd_vel = Twist()
|
||||
self.gazebo_imu = Imu()
|
||||
self.robot_state_subscriber_msg = RobotStateMsg()
|
||||
self.robot_command_publisher_msg = RobotCommandMsg()
|
||||
|
||||
# start ros node
|
||||
rospy.init_node("rl_sim")
|
||||
self.ros_namespace = self.get_namespace()
|
||||
|
||||
# get params from param_node
|
||||
self.param_client = self.create_client(GetParameters, '/param_node/get_parameters')
|
||||
while not self.param_client.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().warn("Waiting for param_node service to be available...")
|
||||
request = GetParameters.Request()
|
||||
request.names = ['robot_name', 'gazebo_model_name']
|
||||
future = self.param_client.call_async(request)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
if len(future.result().values) < 2:
|
||||
self.get_logger().warn("Failed to get all parameters from param_node")
|
||||
else:
|
||||
self.robot_name = future.result().values[0].string_value
|
||||
self.gazebo_model_name = future.result().values[1].string_value
|
||||
self.get_logger().info(f"Get param robot_name: {self.robot_name}")
|
||||
self.get_logger().info(f"Get param gazebo_model_name: {self.gazebo_model_name}")
|
||||
|
||||
# read params from yaml
|
||||
self.robot_name = rospy.get_param("robot_name", "")
|
||||
self.ReadYaml(self.robot_name)
|
||||
for i in range(len(self.params.observations)):
|
||||
if self.params.observations[i] == "ang_vel":
|
||||
self.params.observations[i] = "ang_vel_world"
|
||||
|
||||
# history
|
||||
# init rl
|
||||
torch.set_grad_enabled(False)
|
||||
if len(self.params.observations_history) != 0:
|
||||
self.history_obs_buf = ObservationBuffer(1, self.params.num_observations, len(self.params.observations_history))
|
||||
|
||||
# Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
|
||||
# the mapping table is established according to the order defined in the YAML file
|
||||
sorted_joint_controller_names = sorted(self.params.joint_controller_names)
|
||||
self.sorted_to_original_index = {}
|
||||
for i in range(len(self.params.joint_controller_names)):
|
||||
self.sorted_to_original_index[sorted_joint_controller_names[i]] = i
|
||||
self.mapped_joint_positions = [0.0] * self.params.num_of_dofs
|
||||
self.mapped_joint_velocities = [0.0] * self.params.num_of_dofs
|
||||
self.mapped_joint_efforts = [0.0] * self.params.num_of_dofs
|
||||
|
||||
# init
|
||||
torch.set_grad_enabled(False)
|
||||
self.joint_publishers_commands = [MotorCommand() for _ in range(self.params.num_of_dofs)]
|
||||
self.robot_command_publisher_msg.motor_command = [MotorCommandMsg() for _ in range(self.params.num_of_dofs)]
|
||||
self.robot_state_subscriber_msg.motor_state = [MotorStateMsg() for _ in range(self.params.num_of_dofs)]
|
||||
self.InitObservations()
|
||||
self.InitOutputs()
|
||||
self.InitControl()
|
||||
self.running_state = STATE.STATE_RL_RUNNING
|
||||
|
||||
# model
|
||||
model_path = os.path.join(os.path.dirname(__file__), f"../models/{self.robot_name}/{self.params.model_name}")
|
||||
model_path = os.path.join(get_package_share_directory('rl_sar'), 'models', self.robot_name, self.params.model_name)
|
||||
self.model = torch.jit.load(model_path)
|
||||
|
||||
# publisher
|
||||
self.ros_namespace = rospy.get_param("ros_namespace", "")
|
||||
self.joint_publishers = {}
|
||||
for i in range(self.params.num_of_dofs):
|
||||
topic_name = f"{self.ros_namespace}{self.params.joint_controller_names[i]}/command"
|
||||
self.joint_publishers[self.params.joint_controller_names[i]] = rospy.Publisher(topic_name, MotorCommand, queue_size=10)
|
||||
self.robot_command_publisher = self.create_publisher(RobotCommandMsg, self.ros_namespace + "robot_joint_controller/command", qos_profile_system_default)
|
||||
|
||||
# subscriber
|
||||
self.cmd_vel_subscriber = rospy.Subscriber("/cmd_vel", Twist, self.CmdvelCallback, queue_size=10)
|
||||
self.model_state_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.ModelStatesCallback, queue_size=10)
|
||||
joint_states_topic = f"{self.ros_namespace}joint_states"
|
||||
self.joint_state_subscriber = rospy.Subscriber(joint_states_topic, JointState, self.JointStatesCallback, queue_size=10)
|
||||
self.cmd_vel_subscriber = self.create_subscription(Twist, "/cmd_vel", self.CmdvelCallback, qos_profile_system_default)
|
||||
self.gazebo_imu_subscriber = self.create_subscription(Imu, "/imu", self.GazeboImuCallback, qos_profile_system_default)
|
||||
self.robot_state_subscriber = self.create_subscription(RobotStateMsg, self.ros_namespace + "robot_joint_controller/state", self.RobotStateCallback, qos_profile_system_default)
|
||||
|
||||
# service
|
||||
self.gazebo_model_name = rospy.get_param("gazebo_model_name", "")
|
||||
self.gazebo_set_model_state_client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
|
||||
self.gazebo_pause_physics_client = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
|
||||
self.gazebo_unpause_physics_client = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
|
||||
self.gazebo_set_model_state_client = self.create_client(SetModelState, "/gazebo/set_model_state")
|
||||
self.gazebo_pause_physics_client = self.create_client(Empty, "/gazebo/pause_physics")
|
||||
self.gazebo_unpause_physics_client = self.create_client(Empty, "/gazebo/unpause_physics")
|
||||
|
||||
# loops
|
||||
self.thread_control = threading.Thread(target=self.ThreadControl)
|
||||
self.thread_rl = threading.Thread(target=self.ThreadRL)
|
||||
self.thread_control.daemon = True
|
||||
self.thread_rl.daemon = True
|
||||
self.thread_control.start()
|
||||
self.thread_rl.start()
|
||||
|
||||
|
|
@ -96,87 +100,82 @@ class RL_Sim(RL):
|
|||
if CSV_LOGGER:
|
||||
self.CSVInit(self.robot_name)
|
||||
|
||||
print(LOGGER.INFO + "RL_Sim start")
|
||||
self.get_logger().info("RL_Sim start")
|
||||
|
||||
def __del__(self):
|
||||
print(LOGGER.INFO + "RL_Sim exit")
|
||||
self.get_logger().info("RL_Sim exit")
|
||||
|
||||
def GetState(self, state):
|
||||
if self.params.framework == "isaacgym":
|
||||
state.imu.quaternion[3] = self.pose.orientation.w
|
||||
state.imu.quaternion[0] = self.pose.orientation.x
|
||||
state.imu.quaternion[1] = self.pose.orientation.y
|
||||
state.imu.quaternion[2] = self.pose.orientation.z
|
||||
state.imu.quaternion[3] = self.gazebo_imu.orientation.w
|
||||
state.imu.quaternion[0] = self.gazebo_imu.orientation.x
|
||||
state.imu.quaternion[1] = self.gazebo_imu.orientation.y
|
||||
state.imu.quaternion[2] = self.gazebo_imu.orientation.z
|
||||
elif self.params.framework == "isaacsim":
|
||||
state.imu.quaternion[0] = self.pose.orientation.w
|
||||
state.imu.quaternion[1] = self.pose.orientation.x
|
||||
state.imu.quaternion[2] = self.pose.orientation.y
|
||||
state.imu.quaternion[3] = self.pose.orientation.z
|
||||
state.imu.quaternion[0] = self.gazebo_imu.orientation.w
|
||||
state.imu.quaternion[1] = self.gazebo_imu.orientation.x
|
||||
state.imu.quaternion[2] = self.gazebo_imu.orientation.y
|
||||
state.imu.quaternion[3] = self.gazebo_imu.orientation.z
|
||||
|
||||
state.imu.gyroscope[0] = self.vel.angular.x
|
||||
state.imu.gyroscope[1] = self.vel.angular.y
|
||||
state.imu.gyroscope[2] = self.vel.angular.z
|
||||
state.imu.gyroscope[0] = self.gazebo_imu.angular_velocity.x
|
||||
state.imu.gyroscope[1] = self.gazebo_imu.angular_velocity.y
|
||||
state.imu.gyroscope[2] = self.gazebo_imu.angular_velocity.z
|
||||
|
||||
# state.imu.accelerometer
|
||||
state.imu.accelerometer[0] = self.gazebo_imu.linear_acceleration.x
|
||||
state.imu.accelerometer[1] = self.gazebo_imu.linear_acceleration.y
|
||||
state.imu.accelerometer[2] = self.gazebo_imu.linear_acceleration.z
|
||||
|
||||
for i in range(self.params.num_of_dofs):
|
||||
state.motor_state.q[i] = self.mapped_joint_positions[i]
|
||||
state.motor_state.dq[i] = self.mapped_joint_velocities[i]
|
||||
state.motor_state.tauEst[i] = self.mapped_joint_efforts[i]
|
||||
state.motor_state.q[i] = self.robot_state_subscriber_msg.motor_state[i].q
|
||||
state.motor_state.dq[i] = self.robot_state_subscriber_msg.motor_state[i].dq
|
||||
state.motor_state.tau_est[i] = self.robot_state_subscriber_msg.motor_state[i].tau_est
|
||||
|
||||
def SetCommand(self, command):
|
||||
for i in range(self.params.num_of_dofs):
|
||||
self.joint_publishers_commands[i].q = command.motor_command.q[i]
|
||||
self.joint_publishers_commands[i].dq = command.motor_command.dq[i]
|
||||
self.joint_publishers_commands[i].kp = command.motor_command.kp[i]
|
||||
self.joint_publishers_commands[i].kd = command.motor_command.kd[i]
|
||||
self.joint_publishers_commands[i].tau = command.motor_command.tau[i]
|
||||
self.robot_command_publisher_msg.motor_command[i].q = float(command.motor_command.q[i])
|
||||
self.robot_command_publisher_msg.motor_command[i].dq = float(command.motor_command.dq[i])
|
||||
self.robot_command_publisher_msg.motor_command[i].kp = float(command.motor_command.kp[i])
|
||||
self.robot_command_publisher_msg.motor_command[i].kd = float(command.motor_command.kd[i])
|
||||
self.robot_command_publisher_msg.motor_command[i].tau = float(command.motor_command.tau[i])
|
||||
|
||||
for i in range(self.params.num_of_dofs):
|
||||
self.joint_publishers[self.params.joint_controller_names[i]].publish(self.joint_publishers_commands[i])
|
||||
self.robot_command_publisher.publish(self.robot_command_publisher_msg)
|
||||
|
||||
def RobotControl(self):
|
||||
if self.control.control_state == STATE.STATE_RESET_SIMULATION:
|
||||
set_model_state = SetModelStateRequest().model_state
|
||||
set_model_state.model_name = self.gazebo_model_name
|
||||
set_model_state.pose.position.z = 1.0
|
||||
set_model_state.reference_frame = "world"
|
||||
self.gazebo_set_model_state_client(set_model_state)
|
||||
self.control.control_state = STATE.STATE_WAITING
|
||||
if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION:
|
||||
if self.simulation_running:
|
||||
self.gazebo_pause_physics_client()
|
||||
print("\r\n" + LOGGER.INFO + "Simulation Stop")
|
||||
else:
|
||||
self.gazebo_unpause_physics_client()
|
||||
print("\r\n" + LOGGER.INFO + "Simulation Start")
|
||||
self.simulation_running = not self.simulation_running
|
||||
self.control.control_state = STATE.STATE_WAITING
|
||||
# NOT AVAILABLE NOW
|
||||
# if self.control.control_state == STATE.STATE_RESET_SIMULATION:
|
||||
# set_model_state = SetModelState.Request()
|
||||
# set_model_state.model_state.model_name = self.gazebo_model_name
|
||||
# set_model_state.model_state.pose.position.z = 1.0
|
||||
# set_model_state.model_state.reference_frame = "world"
|
||||
# self.gazebo_set_model_state_client.call_async(set_model_state)
|
||||
# self.control.control_state = STATE.STATE_WAITING
|
||||
# if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION:
|
||||
# if self.simulation_running:
|
||||
# self.gazebo_pause_physics_client.call_async(Empty.Request())
|
||||
# self.get_logger().info("Simulation Stop")
|
||||
# else:
|
||||
# self.gazebo_unpause_physics_client.call_async(Empty.Request())
|
||||
# self.get_logger().info("Simulation Start")
|
||||
# self.simulation_running = not self.simulation_running
|
||||
# self.control.control_state = STATE.STATE_WAITING
|
||||
|
||||
if self.simulation_running:
|
||||
self.GetState(self.robot_state)
|
||||
self.StateController(self.robot_state, self.robot_command)
|
||||
self.SetCommand(self.robot_command)
|
||||
|
||||
def ModelStatesCallback(self, msg):
|
||||
self.vel = msg.twist[2]
|
||||
self.pose = msg.pose[2]
|
||||
def GazeboImuCallback(self, msg):
|
||||
self.gazebo_imu = msg
|
||||
|
||||
def CmdvelCallback(self, msg):
|
||||
self.cmd_vel = msg
|
||||
|
||||
def MapData(self, source_data, target_data):
|
||||
for i in range(len(source_data)):
|
||||
target_data[i] = source_data[self.sorted_to_original_index[self.params.joint_controller_names[i]]]
|
||||
|
||||
def JointStatesCallback(self, msg):
|
||||
self.MapData(msg.position, self.mapped_joint_positions)
|
||||
self.MapData(msg.velocity, self.mapped_joint_velocities)
|
||||
self.MapData(msg.effort, self.mapped_joint_efforts)
|
||||
def RobotStateCallback(self, msg):
|
||||
self.robot_state_subscriber_msg = msg
|
||||
|
||||
def RunModel(self):
|
||||
if self.running_state == STATE.STATE_RL_RUNNING and self.simulation_running:
|
||||
self.obs.lin_vel = torch.tensor([[self.vel.linear.x, self.vel.linear.y, self.vel.linear.z]])
|
||||
# self.obs.lin_vel NOT USE
|
||||
self.obs.ang_vel = torch.tensor(self.robot_state.imu.gyroscope).unsqueeze(0)
|
||||
# self.obs.commands = torch.tensor([[self.cmd_vel.linear.x, self.cmd_vel.linear.y, self.cmd_vel.angular.z]])
|
||||
self.obs.commands = torch.tensor([[self.control.x, self.control.y, self.control.yaw]])
|
||||
|
|
@ -199,7 +198,9 @@ class RL_Sim(RL):
|
|||
self.output_dof_pos = self.ComputePosition(self.obs.actions)
|
||||
|
||||
if CSV_LOGGER:
|
||||
tau_est = torch.tensor(self.mapped_joint_efforts).unsqueeze(0)
|
||||
tau_est = torch.empty(1, self.params.num_of_dofs, dtype=torch.float32)
|
||||
for i in range(self.params.num_of_dofs):
|
||||
tau_est[0, i] = self.robot_state_subscriber_msg.motor_state[i].tau_est
|
||||
self.CSVLogger(self.output_torques, tau_est, self.obs.dof_pos, self.output_dof_pos, self.obs.dof_vel)
|
||||
|
||||
def Forward(self):
|
||||
|
|
@ -219,21 +220,27 @@ class RL_Sim(RL):
|
|||
def ThreadControl(self):
|
||||
thread_period = self.params.dt
|
||||
thread_name = "thread_control"
|
||||
print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
|
||||
while not rospy.is_shutdown():
|
||||
self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
|
||||
while rclpy.ok():
|
||||
self.RobotControl()
|
||||
time.sleep(thread_period)
|
||||
print("[Thread End] named: " + thread_name)
|
||||
self.get_logger().info("[Thread End] named: " + thread_name)
|
||||
|
||||
def ThreadRL(self):
|
||||
thread_period = self.params.dt * self.params.decimation
|
||||
thread_name = "thread_rl"
|
||||
print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
|
||||
while not rospy.is_shutdown():
|
||||
self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
|
||||
while rclpy.ok():
|
||||
self.RunModel()
|
||||
time.sleep(thread_period)
|
||||
print("[Thread End] named: " + thread_name)
|
||||
self.get_logger().info("[Thread End] named: " + thread_name)
|
||||
|
||||
if __name__ == "__main__":
|
||||
rclpy.init()
|
||||
rl_sim = RL_Sim()
|
||||
rospy.spin()
|
||||
try:
|
||||
rclpy.spin(rl_sim)
|
||||
except KeyboardInterrupt:
|
||||
rl_sim.get_logger().info("Shutdown signal received, shutting down...")
|
||||
rclpy.shutdown()
|
||||
|
||||
|
|
|
|||
|
|
@ -116,7 +116,7 @@ void RL_Real::GetState(RobotState<double> *state)
|
|||
{
|
||||
state->motor_state.q[i] = this->unitree_low_state.motorState[state_mapping[i]].q;
|
||||
state->motor_state.dq[i] = this->unitree_low_state.motorState[state_mapping[i]].dq;
|
||||
state->motor_state.tauEst[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst;
|
||||
state->motor_state.tau_est[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -178,7 +178,7 @@ void RL_Real::RunModel()
|
|||
this->output_dof_pos = this->ComputePosition(this->obs.actions);
|
||||
|
||||
#ifdef CSV_LOGGER
|
||||
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
|
||||
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0);
|
||||
this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -122,7 +122,7 @@ void RL_Real::GetState(RobotState<double> *state)
|
|||
{
|
||||
state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q();
|
||||
state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq();
|
||||
state->motor_state.tauEst[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
|
||||
state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -183,7 +183,7 @@ void RL_Real::RunModel()
|
|||
this->output_dof_pos = this->ComputePosition(this->obs.actions);
|
||||
|
||||
#ifdef CSV_LOGGER
|
||||
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
|
||||
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0);
|
||||
this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,14 +1,43 @@
|
|||
#include "rl_sim.hpp"
|
||||
|
||||
// #define PLOT
|
||||
// #define CSV_LOGGER
|
||||
#define CSV_LOGGER
|
||||
|
||||
RL_Sim::RL_Sim()
|
||||
: rclcpp::Node("rl_sim_node")
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
this->ros_namespace = this->get_namespace();
|
||||
|
||||
// get params from param_node
|
||||
param_client = this->create_client<rcl_interfaces::srv::GetParameters>("/param_node/get_parameters");
|
||||
while (!param_client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
std::cout << LOGGER::WARNING << "Waiting for param_node service to be available..." << std::endl;
|
||||
}
|
||||
auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
|
||||
request->names.push_back("robot_name");
|
||||
request->names.push_back("gazebo_model_name");
|
||||
auto future = param_client->async_send_request(request);
|
||||
rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
|
||||
if (future.get()->values.size() < 2)
|
||||
{
|
||||
std::cout << LOGGER::WARNING << "Failed to get all parameters from param_node" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->robot_name = future.get()->values[0].string_value;
|
||||
this->gazebo_model_name = future.get()->values[1].string_value;
|
||||
std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl;
|
||||
std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl;
|
||||
}
|
||||
|
||||
// // get params from this node
|
||||
// this->declare_parameter<std::string>("robot_name", "");
|
||||
// this->get_parameter("robot_name", this->robot_name);
|
||||
// this->declare_parameter<std::string>("gazebo_model_name", "");
|
||||
// this->get_parameter("gazebo_model_name", this->gazebo_model_name);
|
||||
|
||||
// read params from yaml
|
||||
nh.param<std::string>("robot_name", this->robot_name, "");
|
||||
this->ReadYaml(this->robot_name);
|
||||
for (std::string &observation : this->params.observations)
|
||||
{
|
||||
|
|
@ -18,17 +47,6 @@ RL_Sim::RL_Sim()
|
|||
observation = "ang_vel_world";
|
||||
}
|
||||
}
|
||||
// Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
|
||||
// the mapping table is established according to the order defined in the YAML file
|
||||
std::vector<std::string> sorted_joint_controller_names = this->params.joint_controller_names;
|
||||
std::sort(sorted_joint_controller_names.begin(), sorted_joint_controller_names.end());
|
||||
for (size_t i = 0; i < this->params.joint_controller_names.size(); ++i)
|
||||
{
|
||||
this->sorted_to_original_index[sorted_joint_controller_names[i]] = i;
|
||||
}
|
||||
this->mapped_joint_positions = std::vector<double>(this->params.num_of_dofs, 0.0);
|
||||
this->mapped_joint_velocities = std::vector<double>(this->params.num_of_dofs, 0.0);
|
||||
this->mapped_joint_efforts = std::vector<double>(this->params.num_of_dofs, 0.0);
|
||||
|
||||
// init rl
|
||||
torch::autograd::GradMode::set_enabled(false);
|
||||
|
|
@ -36,7 +54,8 @@ RL_Sim::RL_Sim()
|
|||
{
|
||||
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
|
||||
}
|
||||
this->joint_publishers_commands.resize(this->params.num_of_dofs);
|
||||
this->robot_command_publisher_msg.motor_command.resize(this->params.num_of_dofs);
|
||||
this->robot_state_subscriber_msg.motor_state.resize(this->params.num_of_dofs);
|
||||
this->InitObservations();
|
||||
this->InitOutputs();
|
||||
this->InitControl();
|
||||
|
|
@ -47,24 +66,26 @@ RL_Sim::RL_Sim()
|
|||
this->model = torch::jit::load(model_path);
|
||||
|
||||
// publisher
|
||||
nh.param<std::string>("ros_namespace", this->ros_namespace, "");
|
||||
for (int i = 0; i < this->params.num_of_dofs; ++i)
|
||||
{
|
||||
// joint need to rename as xxx_joint
|
||||
this->joint_publishers[this->params.joint_controller_names[i]] =
|
||||
nh.advertise<robot_msgs::MotorCommand>(this->ros_namespace + this->params.joint_controller_names[i] + "/command", 10);
|
||||
}
|
||||
this->robot_command_publisher = this->create_publisher<robot_msgs::msg::RobotCommand>(
|
||||
this->ros_namespace + "robot_joint_controller/command", rclcpp::SystemDefaultsQoS());
|
||||
|
||||
// subscriber
|
||||
this->cmd_vel_subscriber = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
|
||||
this->model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
|
||||
this->joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
|
||||
this->cmd_vel_subscriber = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
"/cmd_vel", rclcpp::SystemDefaultsQoS(),
|
||||
[this] (const geometry_msgs::msg::Twist::SharedPtr msg) {this->CmdvelCallback(msg);}
|
||||
);
|
||||
this->gazebo_imu_subscriber = this->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/imu", rclcpp::SystemDefaultsQoS(), [this] (const sensor_msgs::msg::Imu::SharedPtr msg) {this->GazeboImuCallback(msg);}
|
||||
);
|
||||
this->robot_state_subscriber = this->create_subscription<robot_msgs::msg::RobotState>(
|
||||
this->ros_namespace + "robot_joint_controller/state", rclcpp::SystemDefaultsQoS(),
|
||||
[this] (const robot_msgs::msg::RobotState::SharedPtr msg) {this->RobotStateCallback(msg);}
|
||||
);
|
||||
|
||||
// service
|
||||
nh.param<std::string>("gazebo_model_name", this->gazebo_model_name, "");
|
||||
this->gazebo_set_model_state_client = nh.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
|
||||
this->gazebo_pause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
|
||||
this->gazebo_unpause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
|
||||
this->gazebo_set_model_state_client = this->create_client<gazebo_msgs::srv::SetModelState>("/gazebo/set_model_state");
|
||||
this->gazebo_pause_physics_client = this->create_client<std_srvs::srv::Empty>("/gazebo/pause_physics");
|
||||
this->gazebo_unpause_physics_client = this->create_client<std_srvs::srv::Empty>("/gazebo/unpause_physics");
|
||||
|
||||
// loop
|
||||
this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Sim::RobotControl, this));
|
||||
|
|
@ -107,30 +128,32 @@ void RL_Sim::GetState(RobotState<double> *state)
|
|||
{
|
||||
if (this->params.framework == "isaacgym")
|
||||
{
|
||||
state->imu.quaternion[3] = this->pose.orientation.w;
|
||||
state->imu.quaternion[0] = this->pose.orientation.x;
|
||||
state->imu.quaternion[1] = this->pose.orientation.y;
|
||||
state->imu.quaternion[2] = this->pose.orientation.z;
|
||||
state->imu.quaternion[3] = this->gazebo_imu.orientation.w;
|
||||
state->imu.quaternion[0] = this->gazebo_imu.orientation.x;
|
||||
state->imu.quaternion[1] = this->gazebo_imu.orientation.y;
|
||||
state->imu.quaternion[2] = this->gazebo_imu.orientation.z;
|
||||
}
|
||||
else if (this->params.framework == "isaacsim")
|
||||
{
|
||||
state->imu.quaternion[0] = this->pose.orientation.w;
|
||||
state->imu.quaternion[1] = this->pose.orientation.x;
|
||||
state->imu.quaternion[2] = this->pose.orientation.y;
|
||||
state->imu.quaternion[3] = this->pose.orientation.z;
|
||||
state->imu.quaternion[0] = this->gazebo_imu.orientation.w;
|
||||
state->imu.quaternion[1] = this->gazebo_imu.orientation.x;
|
||||
state->imu.quaternion[2] = this->gazebo_imu.orientation.y;
|
||||
state->imu.quaternion[3] = this->gazebo_imu.orientation.z;
|
||||
}
|
||||
|
||||
state->imu.gyroscope[0] = this->vel.angular.x;
|
||||
state->imu.gyroscope[1] = this->vel.angular.y;
|
||||
state->imu.gyroscope[2] = this->vel.angular.z;
|
||||
state->imu.gyroscope[0] = this->gazebo_imu.angular_velocity.x;
|
||||
state->imu.gyroscope[1] = this->gazebo_imu.angular_velocity.y;
|
||||
state->imu.gyroscope[2] = this->gazebo_imu.angular_velocity.z;
|
||||
|
||||
// state->imu.accelerometer
|
||||
state->imu.accelerometer[0] = this->gazebo_imu.linear_acceleration.x;
|
||||
state->imu.accelerometer[1] = this->gazebo_imu.linear_acceleration.y;
|
||||
state->imu.accelerometer[2] = this->gazebo_imu.linear_acceleration.z;
|
||||
|
||||
for (int i = 0; i < this->params.num_of_dofs; ++i)
|
||||
{
|
||||
state->motor_state.q[i] = this->mapped_joint_positions[i];
|
||||
state->motor_state.dq[i] = this->mapped_joint_velocities[i];
|
||||
state->motor_state.tauEst[i] = this->mapped_joint_efforts[i];
|
||||
state->motor_state.q[i] = this->robot_state_subscriber_msg.motor_state[i].q;
|
||||
state->motor_state.dq[i] = this->robot_state_subscriber_msg.motor_state[i].dq;
|
||||
state->motor_state.tau_est[i] = this->robot_state_subscriber_msg.motor_state[i].tau_est;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -138,49 +161,79 @@ void RL_Sim::SetCommand(const RobotCommand<double> *command)
|
|||
{
|
||||
for (int i = 0; i < this->params.num_of_dofs; ++i)
|
||||
{
|
||||
this->joint_publishers_commands[i].q = command->motor_command.q[i];
|
||||
this->joint_publishers_commands[i].dq = command->motor_command.dq[i];
|
||||
this->joint_publishers_commands[i].kp = command->motor_command.kp[i];
|
||||
this->joint_publishers_commands[i].kd = command->motor_command.kd[i];
|
||||
this->joint_publishers_commands[i].tau = command->motor_command.tau[i];
|
||||
this->robot_command_publisher_msg.motor_command[i].q = command->motor_command.q[i];
|
||||
this->robot_command_publisher_msg.motor_command[i].dq = command->motor_command.dq[i];
|
||||
this->robot_command_publisher_msg.motor_command[i].kp = command->motor_command.kp[i];
|
||||
this->robot_command_publisher_msg.motor_command[i].kd = command->motor_command.kd[i];
|
||||
this->robot_command_publisher_msg.motor_command[i].tau = command->motor_command.tau[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < this->params.num_of_dofs; ++i)
|
||||
{
|
||||
this->joint_publishers[this->params.joint_controller_names[i]].publish(this->joint_publishers_commands[i]);
|
||||
}
|
||||
this->robot_command_publisher->publish(this->robot_command_publisher_msg);
|
||||
}
|
||||
|
||||
void RL_Sim::RobotControl()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(robot_state_mutex);
|
||||
|
||||
if (this->control.control_state == STATE_RESET_SIMULATION)
|
||||
{
|
||||
gazebo_msgs::SetModelState set_model_state;
|
||||
set_model_state.request.model_state.model_name = this->gazebo_model_name;
|
||||
set_model_state.request.model_state.pose.position.z = 1.0;
|
||||
set_model_state.request.model_state.reference_frame = "world";
|
||||
this->gazebo_set_model_state_client.call(set_model_state);
|
||||
|
||||
this->control.control_state = STATE_WAITING;
|
||||
}
|
||||
if (this->control.control_state == STATE_TOGGLE_SIMULATION)
|
||||
{
|
||||
std_srvs::Empty empty;
|
||||
if (simulation_running)
|
||||
{
|
||||
this->gazebo_pause_physics_client.call(empty);
|
||||
std::cout << std::endl << LOGGER::INFO << "Simulation Stop" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->gazebo_unpause_physics_client.call(empty);
|
||||
std::cout << std::endl << LOGGER::INFO << "Simulation Start" << std::endl;
|
||||
}
|
||||
simulation_running = !simulation_running;
|
||||
this->control.control_state = STATE_WAITING;
|
||||
}
|
||||
// if (this->control.control_state == STATE_RESET_SIMULATION)
|
||||
// {
|
||||
// auto set_model_state_request = std::make_shared<gazebo_msgs::srv::SetModelState::Request>();
|
||||
// set_model_state_request->model_state.model_name = this->gazebo_model_name;
|
||||
// set_model_state_request->model_state.pose.position.z = 1.0;
|
||||
// set_model_state_request->model_state.reference_frame = "world";
|
||||
// this->gazebo_set_model_state_client->async_send_request(set_model_state_request,
|
||||
// [this](rclcpp::Client<gazebo_msgs::srv::SetModelState>::SharedFuture future)
|
||||
// {
|
||||
// if (future.get())
|
||||
// {
|
||||
// std::cout << LOGGER::INFO << "Reset Simulation" << std::endl;
|
||||
// this->control.control_state = STATE_WAITING;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// std::cout << LOGGER::WARNING << "Failed to reset simulation" << std::endl;
|
||||
// }
|
||||
// }
|
||||
// );
|
||||
// }
|
||||
// if (this->control.control_state == STATE_TOGGLE_SIMULATION)
|
||||
// {
|
||||
// auto empty_request = std::make_shared<std_srvs::srv::Empty::Request>();
|
||||
// if (simulation_running)
|
||||
// {
|
||||
// this->gazebo_pause_physics_client->async_send_request(empty_request,
|
||||
// [this](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future)
|
||||
// {
|
||||
// if (future.get())
|
||||
// {
|
||||
// std::cout << LOGGER::INFO << "Simulation Stop" << std::endl;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl;
|
||||
// }
|
||||
// }
|
||||
// );
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// this->gazebo_unpause_physics_client->async_send_request(empty_request,
|
||||
// [this](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future)
|
||||
// {
|
||||
// if (future.get())
|
||||
// {
|
||||
// std::cout << LOGGER::INFO << "Simulation Start" << std::endl;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl;
|
||||
// }
|
||||
// }
|
||||
// );
|
||||
// }
|
||||
// simulation_running = !simulation_running;
|
||||
// this->control.control_state = STATE_WAITING;
|
||||
// }
|
||||
if (simulation_running)
|
||||
{
|
||||
this->motiontime++;
|
||||
|
|
@ -190,30 +243,19 @@ void RL_Sim::RobotControl()
|
|||
}
|
||||
}
|
||||
|
||||
void RL_Sim::ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg)
|
||||
void RL_Sim::GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
{
|
||||
this->vel = msg->twist[2];
|
||||
this->pose = msg->pose[2];
|
||||
this->gazebo_imu = *msg;
|
||||
}
|
||||
|
||||
void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg)
|
||||
void RL_Sim::CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
this->cmd_vel = *msg;
|
||||
}
|
||||
|
||||
void RL_Sim::MapData(const std::vector<double> &source_data, std::vector<double> &target_data)
|
||||
void RL_Sim::RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg)
|
||||
{
|
||||
for (size_t i = 0; i < source_data.size(); ++i)
|
||||
{
|
||||
target_data[i] = source_data[this->sorted_to_original_index[this->params.joint_controller_names[i]]];
|
||||
}
|
||||
}
|
||||
|
||||
void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
|
||||
{
|
||||
MapData(msg->position, this->mapped_joint_positions);
|
||||
MapData(msg->velocity, this->mapped_joint_velocities);
|
||||
MapData(msg->effort, this->mapped_joint_efforts);
|
||||
this->robot_state_subscriber_msg = *msg;
|
||||
}
|
||||
|
||||
void RL_Sim::RunModel()
|
||||
|
|
@ -222,7 +264,7 @@ void RL_Sim::RunModel()
|
|||
|
||||
if (this->running_state == STATE_RL_RUNNING && simulation_running)
|
||||
{
|
||||
this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}});
|
||||
// this->obs.lin_vel NOT USE
|
||||
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
|
||||
// this->obs.commands = torch::tensor({{this->cmd_vel.linear.x, this->cmd_vel.linear.y, this->cmd_vel.angular.z}});
|
||||
this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
|
||||
|
|
@ -247,7 +289,11 @@ void RL_Sim::RunModel()
|
|||
this->output_dof_pos = this->ComputePosition(this->obs.actions);
|
||||
|
||||
#ifdef CSV_LOGGER
|
||||
torch::Tensor tau_est = torch::tensor(this->mapped_joint_efforts).unsqueeze(0);
|
||||
torch::Tensor tau_est = torch::empty({1, this->params.num_of_dofs}, torch::kFloat);
|
||||
for (int i = 0; i < this->params.num_of_dofs; ++i)
|
||||
{
|
||||
tau_est[0][i] = this->robot_state_subscriber_msg.motor_state[i].tau_est;
|
||||
}
|
||||
this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -291,8 +337,8 @@ void RL_Sim::Plot()
|
|||
{
|
||||
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->mapped_joint_positions[i]);
|
||||
this->plot_target_joint_pos[i].push_back(this->joint_publishers_commands[i].q);
|
||||
this->plot_real_joint_pos[i].push_back(this->robot_state_subscriber_msg.motor_state[i].q);
|
||||
this->plot_target_joint_pos[i].push_back(this->robot_command_publisher_msg.motor_command[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");
|
||||
|
|
@ -302,17 +348,10 @@ void RL_Sim::Plot()
|
|||
plt::pause(0.01);
|
||||
}
|
||||
|
||||
void signalHandler(int signum)
|
||||
{
|
||||
ros::shutdown();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
signal(SIGINT, signalHandler);
|
||||
ros::init(argc, argv, "rl_sar");
|
||||
RL_Sim rl_sar;
|
||||
ros::spin();
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<RL_Sim>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,31 +1,66 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(robot_joint_controller)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
controller_interface
|
||||
hardware_interface
|
||||
pluginlib
|
||||
roscpp
|
||||
realtime_tools
|
||||
robot_msgs
|
||||
if(DEFINED ENV{ROS_DISTRO})
|
||||
set(ROS_DISTRO_ENV $ENV{ROS_DISTRO})
|
||||
if(ROS_DISTRO_ENV STREQUAL "foxy")
|
||||
add_definitions(-DROS_DISTRO_FOXY)
|
||||
elseif(ROS_DISTRO_ENV STREQUAL "humble")
|
||||
add_definitions(-DROS_DISTRO_HUMBLE)
|
||||
else()
|
||||
add_definitions(-DROS_DISTRO_UNKNOWN)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
find_package(control_toolbox REQUIRED)
|
||||
find_package(realtime_tools REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(controller_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(robot_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(rcl_interfaces REQUIRED)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/robot_joint_controller.cpp
|
||||
src/robot_joint_controller_group.cpp
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
robot_msgs
|
||||
controller_interface
|
||||
hardware_interface
|
||||
pluginlib
|
||||
roscpp
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
target_include_directories(${PROJECT_NAME} PRIVATE include)
|
||||
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
rclcpp
|
||||
urdf
|
||||
control_toolbox
|
||||
realtime_tools
|
||||
hardware_interface
|
||||
controller_interface
|
||||
pluginlib
|
||||
robot_msgs
|
||||
geometry_msgs
|
||||
rcl_interfaces
|
||||
)
|
||||
|
||||
include_directories(include ${catkin_INCLUDE_DIRS})
|
||||
pluginlib_export_plugin_description_file(controller_interface robot_joint_controller_plugins.xml)
|
||||
|
||||
link_directories(${catkin_LIB_DIRS} lib)
|
||||
|
||||
add_library(robot_joint_controller
|
||||
src/robot_joint_controller.cpp
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/${PROJECT_NAME}
|
||||
)
|
||||
target_link_libraries(robot_joint_controller ${catkin_LIBRARIES})
|
||||
|
||||
ament_export_libraries(${PROJECT_NAME})
|
||||
ament_export_dependencies(rclcpp urdf control_toolbox realtime_tools hardware_interface controller_interface pluginlib robot_msgs geometry_msgs rcl_interfaces)
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT ${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
INCLUDES DESTINATION include
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -1,72 +0,0 @@
|
|||
#ifndef ROBOT_JOINT_CONTROLLER_H
|
||||
#define ROBOT_JOINT_CONTROLLER_H
|
||||
|
||||
#include <ros/node_handle.h>
|
||||
#include <urdf/model.h>
|
||||
#include <control_toolbox/pid.h>
|
||||
#include <realtime_tools/realtime_publisher.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <controller_interface/controller.h>
|
||||
#include <std_msgs/Float64.h>
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
#include <controller_interface/controller.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include "robot_msgs/MotorCommand.h"
|
||||
#include "robot_msgs/MotorState.h"
|
||||
#include <geometry_msgs/WrenchStamped.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <math.h>
|
||||
|
||||
#define PosStopF (2.146E+9f)
|
||||
#define VelStopF (16000.0f)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode;
|
||||
double pos;
|
||||
double posStiffness;
|
||||
double vel;
|
||||
double velStiffness;
|
||||
double torque;
|
||||
} ServoCommand;
|
||||
|
||||
namespace robot_joint_controller
|
||||
{
|
||||
class RobotJointController : public controller_interface::Controller<hardware_interface::EffortJointInterface>
|
||||
{
|
||||
private:
|
||||
hardware_interface::JointHandle joint;
|
||||
ros::Subscriber sub_command, sub_ft;
|
||||
control_toolbox::Pid pid_controller_;
|
||||
std::unique_ptr<realtime_tools::RealtimePublisher<robot_msgs::MotorState>> controller_state_publisher_;
|
||||
|
||||
public:
|
||||
std::string name_space;
|
||||
std::string joint_name;
|
||||
urdf::JointConstSharedPtr joint_urdf;
|
||||
realtime_tools::RealtimeBuffer<robot_msgs::MotorCommand> command;
|
||||
robot_msgs::MotorCommand lastCommand;
|
||||
robot_msgs::MotorState lastState;
|
||||
ServoCommand servoCommand;
|
||||
|
||||
RobotJointController();
|
||||
~RobotJointController();
|
||||
virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
|
||||
virtual void starting(const ros::Time &time);
|
||||
virtual void update(const ros::Time &time, const ros::Duration &period);
|
||||
virtual void stopping();
|
||||
void setCommandCB(const robot_msgs::MotorCommandConstPtr &msg);
|
||||
void positionLimits(double &position);
|
||||
void velocityLimits(double &velocity);
|
||||
void effortLimits(double &effort);
|
||||
|
||||
void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
|
||||
void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
|
||||
void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
|
||||
};
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#ifndef ROBOT_JOINT_CONTROLLER_HPP
|
||||
#define ROBOT_JOINT_CONTROLLER_HPP
|
||||
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
#include <urdf/model.h>
|
||||
#include <realtime_tools/realtime_publisher.h>
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
#include "controller_interface/controller_interface.hpp"
|
||||
#include <std_msgs/msg/float64.hpp>
|
||||
#include <rcl_interfaces/srv/get_parameters.hpp>
|
||||
#include "robot_msgs/msg/motor_command.hpp"
|
||||
#include "robot_msgs/msg/motor_state.hpp"
|
||||
#include "visibility_control.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <math.h>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
|
||||
#define PosStopF (2.146E+9f)
|
||||
#define VelStopF (16000.0f)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode;
|
||||
double pos;
|
||||
double pos_stiffness;
|
||||
double vel;
|
||||
double vel_stiffness;
|
||||
double torque;
|
||||
} ServoCommand;
|
||||
|
||||
namespace robot_joint_controller
|
||||
{
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
class RobotJointController : public controller_interface::ControllerInterface
|
||||
{
|
||||
public:
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
RobotJointController();
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::return_type update() override;
|
||||
#elif defined(ROS_DISTRO_HUMBLE)
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
|
||||
#endif
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
|
||||
// ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
// CallbackReturn on_init() override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
void UpdateFunc(const double &period_seconds);
|
||||
void SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg);
|
||||
void PositionLimit(double &position);
|
||||
void VelocityLimit(double &velocity);
|
||||
void EffortLimit(double &effort);
|
||||
|
||||
protected:
|
||||
std::string name_space_;
|
||||
std::string joint_name_;
|
||||
realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand> rt_command_ptr_;
|
||||
robot_msgs::msg::MotorCommand last_command_;
|
||||
robot_msgs::msg::MotorState last_state_;
|
||||
ServoCommand servo_command_;
|
||||
urdf::JointConstSharedPtr joints_urdf_;
|
||||
rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr robot_description_client_;
|
||||
rclcpp::Subscription<robot_msgs::msg::MotorCommand>::SharedPtr joints_command_subscriber_;
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<robot_msgs::msg::MotorState>> controller_state_publisher_;
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
rclcpp::Time previous_update_timestamp_{0};
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace robot_joint_controller
|
||||
|
||||
#endif // ROBOT_JOINT_CONTROLLER_HPP
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#ifndef ROBOT_JOINT_CONTROLLER_GROUP_HPP
|
||||
#define ROBOT_JOINT_CONTROLLER_GROUP_HPP
|
||||
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
#include <urdf/model.h>
|
||||
#include <realtime_tools/realtime_publisher.h>
|
||||
#include <realtime_tools/realtime_buffer.h>
|
||||
#include "controller_interface/controller_interface.hpp"
|
||||
#include <std_msgs/msg/float64.hpp>
|
||||
#include <rcl_interfaces/srv/get_parameters.hpp>
|
||||
#include "robot_msgs/msg/robot_command.hpp"
|
||||
#include "robot_msgs/msg/robot_state.hpp"
|
||||
#include "visibility_control.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <math.h>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
|
||||
#define PosStopF (2.146E+9f)
|
||||
#define VelStopF (16000.0f)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode;
|
||||
double pos;
|
||||
double pos_stiffness;
|
||||
double vel;
|
||||
double vel_stiffness;
|
||||
double torque;
|
||||
} ServoCommand;
|
||||
|
||||
namespace robot_joint_controller
|
||||
{
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
class RobotJointControllerGroup : public controller_interface::ControllerInterface
|
||||
{
|
||||
public:
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
RobotJointControllerGroup();
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::return_type update() override;
|
||||
#elif defined(ROS_DISTRO_HUMBLE)
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
|
||||
#endif
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
|
||||
// ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
// CallbackReturn on_init() override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
void UpdateFunc(const double &period_seconds);
|
||||
void SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg);
|
||||
void PositionLimit(double &position, int &index);
|
||||
void VelocityLimit(double &velocity, int &index);
|
||||
void EffortLimit(double &effort, int &index);
|
||||
|
||||
protected:
|
||||
std::string name_space_;
|
||||
std::vector<std::string> joint_names_;
|
||||
realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand> rt_command_ptr_;
|
||||
robot_msgs::msg::RobotCommand last_command_;
|
||||
robot_msgs::msg::RobotState last_state_;
|
||||
ServoCommand servo_command_;
|
||||
std::vector<urdf::JointConstSharedPtr> joints_urdf_;
|
||||
rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr robot_description_client_;
|
||||
rclcpp::Subscription<robot_msgs::msg::RobotCommand>::SharedPtr joints_command_subscriber_;
|
||||
std::shared_ptr<realtime_tools::RealtimePublisher<robot_msgs::msg::RobotState>> controller_state_publisher_;
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
rclcpp::Time previous_update_timestamp_{0};
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace robot_joint_controller
|
||||
|
||||
#endif // ROBOT_JOINT_CONTROLLER_GROUP_HPP
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_
|
||||
#define ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((dllexport))
|
||||
#define ROBOT_JOINT_CONTROLLER_IMPORT __attribute__((dllimport))
|
||||
#else
|
||||
#define ROBOT_JOINT_CONTROLLER_EXPORT __declspec(dllexport)
|
||||
#define ROBOT_JOINT_CONTROLLER_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef ROBOT_JOINT_CONTROLLER_BUILDING_DLL
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_EXPORT
|
||||
#else
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_IMPORT
|
||||
#endif
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
#define ROBOT_JOINT_CONTROLLER_LOCAL
|
||||
#else
|
||||
#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((visibility("default")))
|
||||
#define ROBOT_JOINT_CONTROLLER_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC __attribute__((visibility("default")))
|
||||
#define ROBOT_JOINT_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
|
||||
#else
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC
|
||||
#define ROBOT_JOINT_CONTROLLER_LOCAL
|
||||
#endif
|
||||
#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_
|
||||
|
|
@ -1,27 +1,27 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>robot_joint_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The robot_joint_controller package</description>
|
||||
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
|
||||
<license>Apache</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>controller_interface</build_depend>
|
||||
<build_depend>hardware_interface</build_depend>
|
||||
<build_depend>pluginlib</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_export_depend>controller_interface</build_export_depend>
|
||||
<build_export_depend>hardware_interface</build_export_depend>
|
||||
<build_export_depend>pluginlib</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<exec_depend>controller_interface</exec_depend>
|
||||
<exec_depend>hardware_interface</exec_depend>
|
||||
<exec_depend>pluginlib</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<depend>robot_msgs</depend>
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
<controller_interface plugin="${prefix}/robot_joint_controller_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
<package format="3">
|
||||
<name>robot_joint_controller</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The robot_joint_controller package</description>
|
||||
|
||||
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>urdf</depend>
|
||||
<depend>control_toolbox</depend>
|
||||
<depend>realtime_tools</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>controller_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>robot_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>rcl_interfaces</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<pluginlib plugin="${prefix}/share/${PROJECT_NAME}/robot_joint_controller_plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,8 +1,12 @@
|
|||
<library path="lib/librobot_joint_controller">
|
||||
<class name="robot_joint_controller/RobotJointController"
|
||||
type="robot_joint_controller::RobotJointController"
|
||||
base_class_type="controller_interface::ControllerBase"/>
|
||||
<description>
|
||||
The robot joint controller.
|
||||
</description>
|
||||
<library path="robot_joint_controller">
|
||||
<class name="robot_joint_controller/RobotJointController"
|
||||
type="robot_joint_controller::RobotJointController"
|
||||
base_class_type="controller_interface::ControllerInterface">
|
||||
<description>The robot joint single controller</description>
|
||||
</class>
|
||||
<class name="robot_joint_controller/RobotJointControllerGroup"
|
||||
type="robot_joint_controller::RobotJointControllerGroup"
|
||||
base_class_type="controller_interface::ControllerInterface">
|
||||
<description>The robot joint group controller</description>
|
||||
</class>
|
||||
</library>
|
||||
|
|
|
|||
|
|
@ -1,198 +1,231 @@
|
|||
#include "robot_joint_controller.h"
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
// #define rqtTune // use rqt or not
|
||||
|
||||
double clamp(double &value, double min, double max)
|
||||
{
|
||||
if (value < min)
|
||||
{
|
||||
value = min;
|
||||
}
|
||||
else if (value > max)
|
||||
{
|
||||
value = max;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
#include "robot_joint_controller.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
namespace robot_joint_controller
|
||||
{
|
||||
using hardware_interface::HW_IF_POSITION;
|
||||
using hardware_interface::HW_IF_VELOCITY;
|
||||
using hardware_interface::HW_IF_EFFORT;
|
||||
|
||||
RobotJointController::RobotJointController()
|
||||
RobotJointController::RobotJointController()
|
||||
: controller_interface::ControllerInterface(),
|
||||
joints_command_subscriber_(nullptr),
|
||||
controller_state_publisher_(nullptr)
|
||||
{
|
||||
memset(&last_command_, 0, sizeof(robot_msgs::msg::MotorCommand));
|
||||
memset(&last_state_, 0, sizeof(robot_msgs::msg::MotorState));
|
||||
memset(&servo_command_, 0, sizeof(ServoCommand));
|
||||
}
|
||||
|
||||
// CallbackReturn RobotJointController::on_init()
|
||||
// {
|
||||
// return CallbackReturn::SUCCESS;
|
||||
// }
|
||||
|
||||
CallbackReturn RobotJointController::on_configure(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
name_space_ = get_node()->get_namespace();
|
||||
|
||||
if (!get_node()->get_parameter("joints", joint_name_))
|
||||
{
|
||||
memset(&lastCommand, 0, sizeof(robot_msgs::MotorCommand));
|
||||
memset(&lastState, 0, sizeof(robot_msgs::MotorState));
|
||||
memset(&servoCommand, 0, sizeof(ServoCommand));
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "No joint given in namespace: '%s'", name_space_.c_str());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(get_node()->get_logger(), "joint_name_: %s", joint_name_.c_str());
|
||||
}
|
||||
|
||||
RobotJointController::~RobotJointController()
|
||||
{
|
||||
sub_ft.shutdown();
|
||||
sub_command.shutdown();
|
||||
}
|
||||
robot_description_client_ = get_node()->create_client<rcl_interfaces::srv::GetParameters>("/robot_state_publisher/get_parameters");
|
||||
|
||||
void RobotJointController::setCommandCB(const robot_msgs::MotorCommandConstPtr &msg)
|
||||
{
|
||||
lastCommand.q = msg->q;
|
||||
lastCommand.kp = msg->kp;
|
||||
lastCommand.dq = msg->dq;
|
||||
lastCommand.kd = msg->kd;
|
||||
lastCommand.tau = msg->tau;
|
||||
// the writeFromNonRT can be used in RT, if you have the guarantee that
|
||||
// * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
|
||||
// * there is only one single rt thread
|
||||
command.writeFromNonRT(lastCommand);
|
||||
}
|
||||
auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
|
||||
request->names.push_back("robot_description_");
|
||||
|
||||
// Controller initialization in non-realtime
|
||||
bool RobotJointController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
|
||||
while (!robot_description_client_->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
name_space = n.getNamespace();
|
||||
if (!n.getParam("joint", joint_name))
|
||||
if (!rclcpp::ok())
|
||||
{
|
||||
ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());
|
||||
return false;
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting.");
|
||||
}
|
||||
RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again...");
|
||||
}
|
||||
|
||||
auto response_received_callback = [this](rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture future)
|
||||
{
|
||||
std::string robot_description = future.get()->values[0].string_value;
|
||||
urdf::Model urdf;
|
||||
if (!urdf.initString(robot_description))
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file");
|
||||
}
|
||||
|
||||
// load pid param from ymal only if rqt need
|
||||
#ifdef rqtTune
|
||||
// Load PID Controller using gains set on parameter server
|
||||
if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
|
||||
return false;
|
||||
joints_urdf_ = urdf.getJoint(joint_name_);
|
||||
if (!joints_urdf_)
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name_.c_str());
|
||||
}
|
||||
};
|
||||
auto future_result = robot_description_client_->async_send_request(request, response_received_callback);
|
||||
|
||||
// Start command subscriber
|
||||
joints_command_subscriber_ = get_node()->create_subscription<robot_msgs::msg::MotorCommand>(
|
||||
"~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointController::SetCommandCallback, this, std::placeholders::_1));
|
||||
|
||||
// Start realtime state publisher
|
||||
controller_state_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<robot_msgs::msg::MotorState>>(
|
||||
get_node()->create_publisher<robot_msgs::msg::MotorState>(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS()));
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
previous_update_timestamp_ = get_node()->get_clock()->now();
|
||||
#endif
|
||||
|
||||
urdf::Model urdf; // Get URDF info about joint
|
||||
if (!urdf.initParamWithNodeHandle("robot_description", n))
|
||||
{
|
||||
ROS_ERROR("Failed to parse urdf file");
|
||||
return false;
|
||||
}
|
||||
joint_urdf = urdf.getJoint(joint_name);
|
||||
if (!joint_urdf)
|
||||
{
|
||||
ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
|
||||
return false;
|
||||
}
|
||||
joint = robot->getHandle(joint_name);
|
||||
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
// Start command subscriber
|
||||
sub_command = n.subscribe("command", 20, &RobotJointController::setCommandCB, this);
|
||||
controller_interface::InterfaceConfiguration RobotJointController::command_interface_configuration() const
|
||||
{
|
||||
controller_interface::InterfaceConfiguration command_interfaces_config;
|
||||
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||
|
||||
// Start realtime state publisher
|
||||
controller_state_publisher_.reset(
|
||||
new realtime_tools::RealtimePublisher<robot_msgs::MotorState>(n, name_space + "/state", 1));
|
||||
command_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT);
|
||||
|
||||
return true;
|
||||
}
|
||||
return command_interfaces_config;
|
||||
|
||||
void RobotJointController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
|
||||
}
|
||||
|
||||
controller_interface::InterfaceConfiguration RobotJointController::state_interface_configuration() const
|
||||
{
|
||||
controller_interface::InterfaceConfiguration state_interfaces_config;
|
||||
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||
|
||||
state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_POSITION);
|
||||
state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT);
|
||||
|
||||
return state_interfaces_config;
|
||||
}
|
||||
|
||||
CallbackReturn RobotJointController::on_activate(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
double init_pos = state_interfaces_[0].get_value();
|
||||
last_command_.q = init_pos;
|
||||
last_state_.q = init_pos;
|
||||
last_command_.dq = 0;
|
||||
last_state_.dq = 0;
|
||||
last_command_.tau = 0;
|
||||
last_state_.tau_est = 0;
|
||||
|
||||
// reset command buffer if a command came through callback when controller was inactive
|
||||
rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand>();
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn RobotJointController::on_deactivate(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
// reset command buffer
|
||||
rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand>();
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
controller_interface::return_type RobotJointController::update()
|
||||
{
|
||||
const auto current_time = get_node()->get_clock()->now();
|
||||
const auto period_seconds = (current_time - previous_update_timestamp_).seconds();
|
||||
previous_update_timestamp_ = current_time;
|
||||
auto joint_command = rt_command_ptr_.readFromRT();
|
||||
// no command received yet
|
||||
if (!joint_command)
|
||||
{
|
||||
pid_controller_.setGains(p, i, d, i_max, i_min, antiwindup);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
|
||||
void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
|
||||
last_command_ = *(joint_command);
|
||||
UpdateFunc(period_seconds);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
#elif defined(ROS_DISTRO_HUMBLE)
|
||||
controller_interface::return_type RobotJointController::update(const rclcpp::Time &time, const rclcpp::Duration &period)
|
||||
{
|
||||
const auto period_seconds = period.seconds();
|
||||
auto joint_command = rt_command_ptr_.readFromRT();
|
||||
// no command received yet
|
||||
if (!joint_command)
|
||||
{
|
||||
pid_controller_.getGains(p, i, d, i_max, i_min, antiwindup);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
|
||||
void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
|
||||
{
|
||||
bool dummy;
|
||||
pid_controller_.getGains(p, i, d, i_max, i_min, dummy);
|
||||
}
|
||||
|
||||
// Controller startup in realtime
|
||||
void RobotJointController::starting(const ros::Time &time)
|
||||
{
|
||||
double init_pos = joint.getPosition();
|
||||
lastCommand.q = init_pos;
|
||||
lastState.q = init_pos;
|
||||
lastCommand.dq = 0;
|
||||
lastState.dq = 0;
|
||||
lastCommand.tau = 0;
|
||||
lastState.tauEst = 0;
|
||||
command.initRT(lastCommand);
|
||||
|
||||
pid_controller_.reset();
|
||||
}
|
||||
|
||||
// Controller update loop in realtime
|
||||
void RobotJointController::update(const ros::Time &time, const ros::Duration &period)
|
||||
{
|
||||
double currentPos, currentVel, calcTorque;
|
||||
lastCommand = *(command.readFromRT());
|
||||
|
||||
// set command data
|
||||
servoCommand.pos = lastCommand.q;
|
||||
positionLimits(servoCommand.pos);
|
||||
servoCommand.posStiffness = lastCommand.kp;
|
||||
if (fabs(lastCommand.q - PosStopF) < 0.00001)
|
||||
{
|
||||
servoCommand.posStiffness = 0;
|
||||
}
|
||||
servoCommand.vel = lastCommand.dq;
|
||||
velocityLimits(servoCommand.vel);
|
||||
servoCommand.velStiffness = lastCommand.kd;
|
||||
if (fabs(lastCommand.dq - VelStopF) < 0.00001)
|
||||
{
|
||||
servoCommand.velStiffness = 0;
|
||||
}
|
||||
servoCommand.torque = lastCommand.tau;
|
||||
effortLimits(servoCommand.torque);
|
||||
|
||||
// rqt set P D gains
|
||||
#ifdef rqtTune
|
||||
double i, i_max, i_min;
|
||||
getGains(servoCommand.posStiffness, i, servoCommand.velStiffness, i_max, i_min);
|
||||
last_command_ = *(joint_command);
|
||||
UpdateFunc(period_seconds);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
#endif
|
||||
|
||||
currentPos = joint.getPosition();
|
||||
// currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());
|
||||
// calcTorque = computeTorque(currentPos, currentVel, servoCommand);
|
||||
currentVel = (currentPos - (double)lastState.q) / period.toSec();
|
||||
calcTorque = servoCommand.posStiffness * (servoCommand.pos - currentPos) + servoCommand.velStiffness * (servoCommand.vel - currentVel) + servoCommand.torque;
|
||||
effortLimits(calcTorque);
|
||||
void RobotJointController::UpdateFunc(const double &period_seconds)
|
||||
{
|
||||
double currentPos = 0.0;
|
||||
double currentVel = 0.0;
|
||||
double calcTorque = 0.0;
|
||||
|
||||
joint.setCommand(calcTorque);
|
||||
|
||||
lastState.q = currentPos;
|
||||
lastState.dq = currentVel;
|
||||
// lastState.tauEst = calcTorque;
|
||||
lastState.tauEst = joint.getEffort();
|
||||
|
||||
// publish state
|
||||
if (controller_state_publisher_ && controller_state_publisher_->trylock())
|
||||
{
|
||||
controller_state_publisher_->msg_.q = lastState.q;
|
||||
controller_state_publisher_->msg_.dq = lastState.dq;
|
||||
controller_state_publisher_->msg_.tauEst = lastState.tauEst;
|
||||
controller_state_publisher_->unlockAndPublish();
|
||||
}
|
||||
}
|
||||
|
||||
// Controller stopping in realtime
|
||||
void RobotJointController::stopping() {}
|
||||
|
||||
void RobotJointController::positionLimits(double &position)
|
||||
// set command data
|
||||
servo_command_.pos = last_command_.q;
|
||||
PositionLimit(servo_command_.pos);
|
||||
servo_command_.pos_stiffness = last_command_.kp;
|
||||
if (fabs(last_command_.q - PosStopF) < 0.00001)
|
||||
{
|
||||
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
|
||||
clamp(position, joint_urdf->limits->lower, joint_urdf->limits->upper);
|
||||
servo_command_.pos_stiffness = 0;
|
||||
}
|
||||
|
||||
void RobotJointController::velocityLimits(double &velocity)
|
||||
servo_command_.vel = last_command_.dq;
|
||||
VelocityLimit(servo_command_.vel);
|
||||
servo_command_.vel_stiffness = last_command_.kd;
|
||||
if (fabs(last_command_.dq - VelStopF) < 0.00001)
|
||||
{
|
||||
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
|
||||
clamp(velocity, -joint_urdf->limits->velocity, joint_urdf->limits->velocity);
|
||||
servo_command_.vel_stiffness = 0;
|
||||
}
|
||||
servo_command_.torque = last_command_.tau;
|
||||
EffortLimit(servo_command_.torque);
|
||||
|
||||
void RobotJointController::effortLimits(double &effort)
|
||||
currentPos = state_interfaces_[0].get_value();
|
||||
currentVel = (currentPos - (double)(last_state_.q)) / period_seconds;
|
||||
calcTorque = servo_command_.pos_stiffness * (servo_command_.pos - currentPos) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel) + servo_command_.torque;
|
||||
EffortLimit(calcTorque);
|
||||
|
||||
command_interfaces_[0].set_value(calcTorque);
|
||||
|
||||
last_state_.q = currentPos;
|
||||
last_state_.dq = currentVel;
|
||||
last_state_.tau_est = state_interfaces_[1].get_value();
|
||||
|
||||
// publish state
|
||||
if (controller_state_publisher_ && controller_state_publisher_->trylock())
|
||||
{
|
||||
if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
|
||||
clamp(effort, -joint_urdf->limits->effort, joint_urdf->limits->effort);
|
||||
controller_state_publisher_->msg_ = last_state_;
|
||||
controller_state_publisher_->unlockAndPublish();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
void RobotJointController::SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg)
|
||||
{
|
||||
last_command_ = *msg;
|
||||
rt_command_ptr_.writeFromNonRT(last_command_);
|
||||
}
|
||||
|
||||
void RobotJointController::PositionLimit(double &position)
|
||||
{
|
||||
std::clamp(position, joints_urdf_->limits->lower, joints_urdf_->limits->upper);
|
||||
}
|
||||
|
||||
void RobotJointController::VelocityLimit(double &velocity)
|
||||
{
|
||||
std::clamp(velocity, -joints_urdf_->limits->velocity, joints_urdf_->limits->velocity);
|
||||
}
|
||||
|
||||
void RobotJointController::EffortLimit(double &effort)
|
||||
{
|
||||
std::clamp(effort, -joints_urdf_->limits->effort, joints_urdf_->limits->effort);
|
||||
}
|
||||
|
||||
} // namespace robot_joint_controller
|
||||
|
||||
// Register controller to pluginlib
|
||||
PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerBase);
|
||||
PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerInterface)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,278 @@
|
|||
#include "robot_joint_controller_group.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
namespace robot_joint_controller
|
||||
{
|
||||
using hardware_interface::HW_IF_POSITION;
|
||||
using hardware_interface::HW_IF_VELOCITY;
|
||||
using hardware_interface::HW_IF_EFFORT;
|
||||
|
||||
RobotJointControllerGroup::RobotJointControllerGroup()
|
||||
: controller_interface::ControllerInterface(),
|
||||
joints_command_subscriber_(nullptr),
|
||||
controller_state_publisher_(nullptr)
|
||||
{
|
||||
memset(&servo_command_, 0, sizeof(ServoCommand));
|
||||
}
|
||||
|
||||
// CallbackReturn RobotJointControllerGroup::on_init()
|
||||
// {
|
||||
// return CallbackReturn::SUCCESS;
|
||||
// }
|
||||
|
||||
CallbackReturn RobotJointControllerGroup::on_configure(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
name_space_ = get_node()->get_namespace();
|
||||
|
||||
joint_names_ = get_node()->get_parameter("joints").as_string_array();
|
||||
|
||||
if (joint_names_.empty())
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (const auto& joint_name : joint_names_)
|
||||
{
|
||||
RCLCPP_WARN(get_node()->get_logger(), "joint_name: %s", joint_name.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
robot_description_client_ = get_node()->create_client<rcl_interfaces::srv::GetParameters>("/robot_state_publisher/get_parameters");
|
||||
|
||||
auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
|
||||
request->names.push_back("robot_description");
|
||||
|
||||
while (!robot_description_client_->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
if (!rclcpp::ok())
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting.");
|
||||
}
|
||||
RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again...");
|
||||
}
|
||||
|
||||
auto response_received_callback = [this](rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture future)
|
||||
{
|
||||
std::string robot_description = future.get()->values[0].string_value;
|
||||
urdf::Model urdf;
|
||||
if (!urdf.initString(robot_description))
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file");
|
||||
}
|
||||
|
||||
for (const auto& joint_name : joint_names_)
|
||||
{
|
||||
auto joint_urdf = urdf.getJoint(joint_name);
|
||||
if (!joint_urdf)
|
||||
{
|
||||
RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name.c_str());
|
||||
}
|
||||
if (joint_urdf)
|
||||
{
|
||||
joints_urdf_.push_back(joint_urdf);
|
||||
}
|
||||
}
|
||||
};
|
||||
auto future_result = robot_description_client_->async_send_request(request, response_received_callback);
|
||||
|
||||
// Start command subscriber
|
||||
joints_command_subscriber_ = get_node()->create_subscription<robot_msgs::msg::RobotCommand>(
|
||||
"~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointControllerGroup::SetCommandCallback, this, std::placeholders::_1));
|
||||
|
||||
// Start realtime state publisher
|
||||
controller_state_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<robot_msgs::msg::RobotState>>(
|
||||
get_node()->create_publisher<robot_msgs::msg::RobotState>(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS()));
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
previous_update_timestamp_ = get_node()->get_clock()->now();
|
||||
#endif
|
||||
|
||||
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
controller_interface::InterfaceConfiguration RobotJointControllerGroup::command_interface_configuration() const
|
||||
{
|
||||
controller_interface::InterfaceConfiguration command_interfaces_config;
|
||||
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||
|
||||
for (const auto & joint_name : joint_names_)
|
||||
{
|
||||
command_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT);
|
||||
}
|
||||
|
||||
return command_interfaces_config;
|
||||
}
|
||||
|
||||
controller_interface::InterfaceConfiguration RobotJointControllerGroup::state_interface_configuration() const
|
||||
{
|
||||
controller_interface::InterfaceConfiguration state_interfaces_config;
|
||||
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||
|
||||
for (const auto & joint_name : joint_names_)
|
||||
{
|
||||
state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_POSITION);
|
||||
state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT);
|
||||
}
|
||||
|
||||
return state_interfaces_config;
|
||||
}
|
||||
|
||||
CallbackReturn RobotJointControllerGroup::on_activate(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
last_command_ = robot_msgs::msg::RobotCommand();
|
||||
last_command_.motor_command.resize(joint_names_.size());
|
||||
last_state_ = robot_msgs::msg::RobotState();
|
||||
last_state_.motor_state.resize(joint_names_.size());
|
||||
|
||||
for (int index = 0; index < joint_names_.size(); ++index)
|
||||
{
|
||||
double init_pos = state_interfaces_[index * 2].get_value();
|
||||
last_command_.motor_command[index].q = init_pos;
|
||||
last_state_.motor_state[index].q = init_pos;
|
||||
last_command_.motor_command[index].dq = 0;
|
||||
last_state_.motor_state[index].dq = 0;
|
||||
last_command_.motor_command[index].tau = 0;
|
||||
last_state_.motor_state[index].tau_est = 0;
|
||||
}
|
||||
|
||||
// reset command buffer if a command came through callback when controller was inactive
|
||||
rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand>();
|
||||
rt_command_ptr_.writeFromNonRT(last_command_);
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
CallbackReturn RobotJointControllerGroup::on_deactivate(const rclcpp_lifecycle::State &previous_state)
|
||||
{
|
||||
last_command_ = robot_msgs::msg::RobotCommand();
|
||||
last_command_.motor_command.resize(joint_names_.size());
|
||||
last_state_ = robot_msgs::msg::RobotState();
|
||||
last_state_.motor_state.resize(joint_names_.size());
|
||||
|
||||
// reset command buffer
|
||||
rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand>();
|
||||
rt_command_ptr_.writeFromNonRT(last_command_);
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
#if defined(ROS_DISTRO_FOXY)
|
||||
controller_interface::return_type RobotJointControllerGroup::update()
|
||||
{
|
||||
const auto current_time = get_node()->get_clock()->now();
|
||||
const auto period_seconds = (current_time - previous_update_timestamp_).seconds();
|
||||
previous_update_timestamp_ = current_time;
|
||||
auto joint_commands = rt_command_ptr_.readFromRT();
|
||||
// no command received yet
|
||||
if (!joint_commands)
|
||||
{
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
if (joint_commands->motor_command.size() != joint_names_.size())
|
||||
{
|
||||
RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
|
||||
"command size (%zu) does not match number of interfaces (%zu)",
|
||||
joint_commands->motor_command.size(), joint_names_.size());
|
||||
return controller_interface::return_type::ERROR;
|
||||
}
|
||||
last_command_ = *(joint_commands);
|
||||
UpdateFunc(period_seconds);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
#elif defined(ROS_DISTRO_HUMBLE)
|
||||
controller_interface::return_type RobotJointControllerGroup::update(const rclcpp::Time &time, const rclcpp::Duration &period)
|
||||
{
|
||||
const auto period_seconds = period.seconds();
|
||||
auto joint_commands = rt_command_ptr_.readFromRT();
|
||||
// no command received yet
|
||||
if (!joint_commands)
|
||||
{
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
if (joint_commands->motor_command.size() != joint_names_.size())
|
||||
{
|
||||
RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
|
||||
"command size (%zu) does not match number of interfaces (%zu)",
|
||||
joint_commands->motor_command.size(), joint_names_.size());
|
||||
return controller_interface::return_type::ERROR;
|
||||
}
|
||||
last_command_ = *(joint_commands);
|
||||
UpdateFunc(period_seconds);
|
||||
return controller_interface::return_type::OK;
|
||||
}
|
||||
#endif
|
||||
|
||||
void RobotJointControllerGroup::UpdateFunc(const double &period_seconds)
|
||||
{
|
||||
std::vector<double> currentPos(joint_names_.size(), 0.0);
|
||||
std::vector<double> currentVel(joint_names_.size(), 0.0);
|
||||
std::vector<double> calcTorque(joint_names_.size(), 0.0);
|
||||
|
||||
for (int index = 0; index < joint_names_.size(); ++index)
|
||||
{
|
||||
// set command data
|
||||
servo_command_.pos = last_command_.motor_command[index].q;
|
||||
PositionLimit(servo_command_.pos, index);
|
||||
servo_command_.pos_stiffness = last_command_.motor_command[index].kp;
|
||||
if (fabs(last_command_.motor_command[index].q - PosStopF) < 0.00001)
|
||||
{
|
||||
servo_command_.pos_stiffness = 0;
|
||||
}
|
||||
servo_command_.vel = last_command_.motor_command[index].dq;
|
||||
VelocityLimit(servo_command_.vel, index);
|
||||
servo_command_.vel_stiffness = last_command_.motor_command[index].kd;
|
||||
if (fabs(last_command_.motor_command[index].dq - VelStopF) < 0.00001)
|
||||
{
|
||||
servo_command_.vel_stiffness = 0;
|
||||
}
|
||||
servo_command_.torque = last_command_.motor_command[index].tau;
|
||||
EffortLimit(servo_command_.torque, index);
|
||||
|
||||
currentPos[index] = state_interfaces_[index * 2].get_value();
|
||||
currentVel[index] = (currentPos[index] - (double)(last_state_.motor_state[index].q)) / period_seconds;
|
||||
calcTorque[index] = servo_command_.pos_stiffness * (servo_command_.pos - currentPos[index]) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel[index]) + servo_command_.torque;
|
||||
EffortLimit(calcTorque[index], index);
|
||||
|
||||
command_interfaces_[index].set_value(calcTorque[index]);
|
||||
|
||||
last_state_.motor_state[index].q = currentPos[index];
|
||||
last_state_.motor_state[index].dq = currentVel[index];
|
||||
last_state_.motor_state[index].tau_est = state_interfaces_[index + 1].get_value();
|
||||
}
|
||||
|
||||
// publish state
|
||||
if (controller_state_publisher_ && controller_state_publisher_->trylock())
|
||||
{
|
||||
controller_state_publisher_->msg_.motor_state = last_state_.motor_state;
|
||||
controller_state_publisher_->unlockAndPublish();
|
||||
}
|
||||
}
|
||||
|
||||
void RobotJointControllerGroup::SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg)
|
||||
{
|
||||
last_command_ = *msg;
|
||||
rt_command_ptr_.writeFromNonRT(last_command_);
|
||||
}
|
||||
|
||||
void RobotJointControllerGroup::PositionLimit(double &position, int &index)
|
||||
{
|
||||
std::clamp(position, joints_urdf_[index]->limits->lower, joints_urdf_[index]->limits->upper);
|
||||
}
|
||||
|
||||
void RobotJointControllerGroup::VelocityLimit(double &velocity, int &index)
|
||||
{
|
||||
std::clamp(velocity, -joints_urdf_[index]->limits->velocity, joints_urdf_[index]->limits->velocity);
|
||||
}
|
||||
|
||||
void RobotJointControllerGroup::EffortLimit(double &effort, int &index)
|
||||
{
|
||||
std::clamp(effort, -joints_urdf_[index]->limits->effort, joints_urdf_[index]->limits->effort);
|
||||
}
|
||||
|
||||
} // namespace robot_joint_controller
|
||||
|
||||
// Register controller to pluginlib
|
||||
PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointControllerGroup, controller_interface::ControllerInterface)
|
||||
|
|
@ -1,44 +1,17 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(robot_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
message_generation
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/MotorCommand.msg"
|
||||
"msg/MotorState.msg"
|
||||
"msg/RobotCommand.msg"
|
||||
"msg/RobotState.msg"
|
||||
"msg/IMU.msg"
|
||||
)
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
MotorCommand.msg
|
||||
MotorState.msg
|
||||
RobotCommand.msg
|
||||
RobotState.msg
|
||||
IMU.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
message_runtime
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# Mark topic names header files for installation
|
||||
install(
|
||||
DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
float32 q # motor current position (rad)
|
||||
float32 dq # motor current speed (rad/s)
|
||||
float32 ddq # motor current speed (rad/s)
|
||||
float32 tauEst # current estimated output torque (N*m)
|
||||
float32 q # motor current pos (rad)
|
||||
float32 dq # motor current vel (rad/s)
|
||||
float32 ddq # motor current acc (rad/s^2)
|
||||
float32 tau_est # current estimated output torque (N*m)
|
||||
float32 cur # current estimated output cur (N*m)
|
||||
|
|
@ -1 +1 @@
|
|||
MotorCommand[32] motor_command
|
||||
MotorCommand[] motor_command
|
||||
|
|
@ -1,2 +1,2 @@
|
|||
IMU imu
|
||||
MotorState[32] motor_state
|
||||
MotorState[] motor_state
|
||||
|
|
@ -1,19 +1,19 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<package format="2">
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robot_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>
|
||||
The robot messgaes package.
|
||||
</description>
|
||||
<description>The robot messages package.</description>
|
||||
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
|
||||
<license>Apache</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>message_runtime</depend>
|
||||
<depend>message_generation</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
||||
</package>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,18 +1,22 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(a1_description)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
genmsg
|
||||
roscpp
|
||||
std_msgs
|
||||
tf
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
find_package(xacro REQUIRED)
|
||||
find_package(robot_state_publisher REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY
|
||||
config
|
||||
launch
|
||||
meshes
|
||||
urdf
|
||||
xacro
|
||||
|
||||
DESTINATION
|
||||
share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
)
|
||||
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -1,70 +1,113 @@
|
|||
a1_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
# use_sim_time: true # If running in simulation
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
type: imu_sensor_broadcaster/ImuSensorBroadcaster
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
ros__parameters:
|
||||
joint: FL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: FL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_calf_controller:
|
||||
ros__parameters:
|
||||
joint: FL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
ros__parameters:
|
||||
joint: FR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: FR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_calf_controller:
|
||||
ros__parameters:
|
||||
joint: FR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
ros__parameters:
|
||||
joint: RL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: RL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_calf_controller:
|
||||
ros__parameters:
|
||||
joint: RL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
ros__parameters:
|
||||
joint: RR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: RR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_calf_controller:
|
||||
ros__parameters:
|
||||
joint: RR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
frame_id: imu_link
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,34 @@
|
|||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
# use_sim_time: true # If running in simulation
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
type: imu_sensor_broadcaster/ImuSensorBroadcaster
|
||||
|
||||
robot_joint_controller:
|
||||
type: robot_joint_controller/RobotJointControllerGroup
|
||||
|
||||
robot_joint_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- FL_hip_joint
|
||||
- FL_thigh_joint
|
||||
- FL_calf_joint
|
||||
- FR_hip_joint
|
||||
- FR_thigh_joint
|
||||
- FR_calf_joint
|
||||
- RL_hip_joint
|
||||
- RL_thigh_joint
|
||||
- RL_calf_joint
|
||||
- RR_hip_joint
|
||||
- RR_thigh_joint
|
||||
- RR_calf_joint
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
frame_id: imu_link
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
<launch>
|
||||
|
||||
<arg name="user_debug" default="false"/>
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find a1_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 a1_description)/launch/check_joint.rviz"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
from launch.conditions import IfCondition
|
||||
|
||||
import xacro
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
####### DATA INPUT ##########
|
||||
# urdf_file = 'moonbotX.urdf'
|
||||
package_description = "a1_description"
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
use_joint_state_publisher = LaunchConfiguration('use_joint_state_publisher')
|
||||
|
||||
# Process the URDF file
|
||||
pkg_path = os.path.join(get_package_share_directory(package_description))
|
||||
xacro_file = os.path.join(pkg_path,'xacro','robot.xacro')
|
||||
robot_description_config = xacro.process_file(xacro_file)
|
||||
|
||||
jsp_arg = DeclareLaunchArgument(
|
||||
'use_joint_state_publisher',
|
||||
default_value='True'
|
||||
)
|
||||
|
||||
# Create a robot_state_publisher node
|
||||
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher_node',
|
||||
emulate_tty=True,
|
||||
output='screen',
|
||||
parameters=[params]
|
||||
)
|
||||
|
||||
# Joint State Publisher
|
||||
joint_state_publisher_node = Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
output='screen',
|
||||
condition=IfCondition(use_joint_state_publisher)
|
||||
)
|
||||
|
||||
# RVIZ Configuration
|
||||
rviz_config_dir = os.path.join(
|
||||
get_package_share_directory(package_description),
|
||||
'launch',
|
||||
'check_joint.rviz'
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
output='screen',
|
||||
name='rviz_node',
|
||||
parameters=[{'use_sim_time': True}],
|
||||
arguments=['-d', rviz_config_dir]
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use sim time if true'),
|
||||
|
||||
jsp_arg,
|
||||
|
||||
# map_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
joint_state_publisher_node,
|
||||
rviz_node
|
||||
|
||||
])
|
||||
|
|
@ -1,38 +1,33 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 420
|
||||
- Class: rviz/Selection
|
||||
Tree Height: 617
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
- Class: rviz_common/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
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
|
|
@ -49,8 +44,16 @@ Visualization Manager:
|
|||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
|
|
@ -174,92 +177,86 @@ Visualization Manager:
|
|||
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
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
- Class: rviz_default_plugins/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
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 1.0307118892669678
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.9769524335861206
|
||||
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
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.0047969818115234
|
||||
Pitch: 0.21039816737174988
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 4.558584690093994
|
||||
Yaw: 0.9653980731964111
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 627
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1108
|
||||
X: 812
|
||||
Y: 241
|
||||
Width: 1200
|
||||
X: 617
|
||||
Y: 102
|
||||
|
|
|
|||
|
|
@ -0,0 +1,59 @@
|
|||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch.event_handlers import OnProcessExit
|
||||
import xacro
|
||||
|
||||
def generate_launch_description():
|
||||
package_name = 'a1_description'
|
||||
package_path = os.path.join(get_package_share_directory(package_name))
|
||||
|
||||
xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro')
|
||||
robot_description = xacro.process_file(xacro_file).toxml()
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name='robot_state_publisher',
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description}],
|
||||
)
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
|
||||
),
|
||||
launch_arguments={'verbose': 'true'}.items()
|
||||
)
|
||||
|
||||
spawn_entity = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
joint_state_broadcaster_node = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["joint_state_broadcaster"],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
robot_joint_controller_node = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["robot_joint_controller"],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
robot_state_publisher_node,
|
||||
gazebo,
|
||||
spawn_entity,
|
||||
joint_state_broadcaster_node,
|
||||
robot_joint_controller_node,
|
||||
])
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch.event_handlers import OnProcessExit
|
||||
import xacro
|
||||
|
||||
def generate_launch_description():
|
||||
package_name = 'a1_description'
|
||||
package_path = os.path.join(get_package_share_directory(package_name))
|
||||
|
||||
xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro')
|
||||
robot_description = xacro.process_file(xacro_file).toxml()
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
name='robot_state_publisher',
|
||||
output="screen",
|
||||
parameters=[{"robot_description": robot_description}],
|
||||
)
|
||||
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
|
||||
),
|
||||
launch_arguments={'verbose': 'true'}.items()
|
||||
)
|
||||
|
||||
spawn_entity = Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
controller_names = [
|
||||
"joint_state_broadcaster",
|
||||
"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"
|
||||
]
|
||||
|
||||
controller_nodes = [Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=[name],
|
||||
output='screen',
|
||||
) for name in controller_names]
|
||||
|
||||
return LaunchDescription([
|
||||
robot_state_publisher_node,
|
||||
gazebo,
|
||||
spawn_entity,
|
||||
*controller_nodes,
|
||||
])
|
||||
|
|
@ -1,14 +1,22 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>a1_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The a1_description package</description>
|
||||
<package format="3">
|
||||
<name>a1_description</name>
|
||||
<version>2.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<maintainer email="laikago@unitree.cc">unitree</maintainer>
|
||||
<license>TODO</license>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>urdf</depend>
|
||||
<depend>xacro</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<gazebo_ros gazebo_model_path="${prefix}/.."/>
|
||||
</export>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -44,12 +44,20 @@
|
|||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<!-- <gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>/a1_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</gazebo> -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
||||
<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
|
||||
<robot_param>robot_description</robot_param>
|
||||
<robot_param_node>robot_state_publisher_node</robot_param_node>
|
||||
<parameters>$(find a1_description)/config/robot_control.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<!-- Show the trajectory of trunk center. -->
|
||||
<gazebo>
|
||||
<plugin filename="libLinkPlot3DPlugin.so" name="3dplotTrunk">
|
||||
|
|
|
|||
|
|
@ -1,15 +1,7 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/a1_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- Show the trajectory of trunk center. -->
|
||||
<gazebo>
|
||||
<!-- <gazebo>
|
||||
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
|
||||
<frequency>10</frequency>
|
||||
<plot>
|
||||
|
|
@ -18,7 +10,7 @@
|
|||
<material>Gazebo/Yellow</material>
|
||||
</plot>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</gazebo> -->
|
||||
|
||||
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
|
||||
<!-- <gazebo>
|
||||
|
|
@ -32,31 +24,80 @@
|
|||
</plugin>
|
||||
</gazebo> -->
|
||||
|
||||
<gazebo>
|
||||
<!-- <gazebo>
|
||||
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
|
||||
<bodyName>trunk</bodyName>
|
||||
<topicName>/apply_force/trunk</topicName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</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>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
</plugin>
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<!-- Foot contacts. -->
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
<xacro:include filename="$(find a1_description)/xacro/leg.xacro"/>
|
||||
<xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
|
||||
<xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
|
||||
<xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
|
||||
<!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/> -->
|
||||
|
||||
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
|
||||
|
|
@ -24,7 +25,7 @@
|
|||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
</joint>
|
||||
</xacro:if>
|
||||
</xacro:if>
|
||||
|
||||
<link name="base">
|
||||
<visual>
|
||||
|
|
@ -61,7 +62,7 @@
|
|||
<inertia
|
||||
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
|
||||
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
|
||||
izz="${trunk_izz}"/>
|
||||
izz="${trunk_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
|
@ -91,10 +92,10 @@
|
|||
<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)}"/>
|
||||
izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</xacro:if>
|
||||
</xacro:if>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,137 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<ros2_control name="GazeboSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="FR_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
||||
<parameters>$(find a1_description)/config/robot_control_group.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -1,14 +1,22 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(go2_description)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
find_package(xacro REQUIRED)
|
||||
find_package(robot_state_publisher REQUIRED)
|
||||
|
||||
catkin_package()
|
||||
install(
|
||||
DIRECTORY
|
||||
config
|
||||
launch
|
||||
meshes
|
||||
urdf
|
||||
xacro
|
||||
|
||||
find_package(roslaunch)
|
||||
DESTINATION
|
||||
share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
foreach(dir config launch meshes urdf)
|
||||
install(DIRECTORY ${dir}/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||
endforeach(dir)
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -1,70 +1,113 @@
|
|||
go2_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
# use_sim_time: true # If running in simulation
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
type: imu_sensor_broadcaster/ImuSensorBroadcaster
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
ros__parameters:
|
||||
joint: FL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: FL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FL_calf_controller:
|
||||
ros__parameters:
|
||||
joint: FL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
ros__parameters:
|
||||
joint: FR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: FR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
FR_calf_controller:
|
||||
ros__parameters:
|
||||
joint: FR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
ros__parameters:
|
||||
joint: RL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RL_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: RL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RL_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RL_calf_controller:
|
||||
ros__parameters:
|
||||
joint: RL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
ros__parameters:
|
||||
joint: RR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RR_thigh_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_thigh_controller:
|
||||
ros__parameters:
|
||||
joint: RR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RR_calf_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
RR_calf_controller:
|
||||
ros__parameters:
|
||||
joint: RR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
imu_sensor_broadcaster:
|
||||
ros__parameters:
|
||||
sensor_name: "imu_sensor"
|
||||
frame_id: imu_link
|
||||
|
||||
|
|
|
|||
|
|
@ -1,21 +1,22 @@
|
|||
<package format="2">
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>go2_description</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
<p>URDF Description package for go2_description</p>
|
||||
<p>This package contains configuration data, 3D models and launch files
|
||||
for go2_description robot</p>
|
||||
</description>
|
||||
<author>TODO</author>
|
||||
<maintainer email="TODO@email.com" />
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roslaunch</depend>
|
||||
<version>2.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>urdf</depend>
|
||||
<depend>xacro</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>rviz</depend>
|
||||
<depend>joint_state_publisher_gui</depend>
|
||||
<depend>gazebo</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<architecture_independent />
|
||||
<build_type>ament_cmake</build_type>
|
||||
<gazebo_ros gazebo_model_path="${prefix}/.."/>
|
||||
</export>
|
||||
</package>
|
||||
</package>
|
||||
|
|
|
|||
|
|
@ -1,13 +1,5 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/go2_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- Show the trajectory of trunk center. -->
|
||||
<!--gazebo>
|
||||
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
|
||||
|
|
@ -21,7 +13,7 @@
|
|||
</gazebo-->
|
||||
|
||||
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
|
||||
<gazebo>
|
||||
<!-- <gazebo>
|
||||
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
|
||||
<frequency>100</frequency>
|
||||
<plot>
|
||||
|
|
@ -29,43 +21,86 @@
|
|||
<pose>0 0 0 0 0 0</pose>
|
||||
</plot>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</gazebo> -->
|
||||
|
||||
<gazebo>
|
||||
<!-- <gazebo>
|
||||
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
|
||||
<bodyName>trunk</bodyName>
|
||||
<topicName>/apply_force/trunk</topicName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</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>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<ros>
|
||||
<namespace>/</namespace>
|
||||
<remapping>~/out:=imu</remapping>
|
||||
</ros>
|
||||
<initial_orientation_as_reference>false</initial_orientation_as_reference>
|
||||
</plugin>
|
||||
<always_on>true</always_on>
|
||||
<update_rate>100</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</x>
|
||||
<y>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</y>
|
||||
<z>
|
||||
<noise type="gaussian">
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</noise>
|
||||
</z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Foot contacts. -->
|
||||
<gazebo reference="FR_calf">
|
||||
<!-- <gazebo reference="FR_calf">
|
||||
<sensor name="FR_foot_contact" type="contact">
|
||||
<update_rate>100</update_rate>
|
||||
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
|
||||
|
|
@ -100,10 +135,10 @@
|
|||
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</gazebo> -->
|
||||
|
||||
<!-- Visualization of Foot contacts. -->
|
||||
<gazebo reference="FR_foot">
|
||||
<!-- <gazebo reference="FR_foot">
|
||||
<visual>
|
||||
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
|
||||
<topicName>FR_foot_contact</topicName>
|
||||
|
|
@ -130,7 +165,7 @@
|
|||
<topicName>RL_foot_contact</topicName>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
</gazebo> -->
|
||||
|
||||
<gazebo reference="base">
|
||||
<turnGravityOff>false</turnGravityOff>
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
<xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
|
||||
<!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
|
||||
<xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
|
||||
<xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
|
||||
<!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->
|
||||
|
||||
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
|
||||
|
|
@ -24,7 +25,7 @@
|
|||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
</joint>
|
||||
</xacro:if>
|
||||
</xacro:if>
|
||||
|
||||
<link name="base">
|
||||
<visual>
|
||||
|
|
@ -61,7 +62,7 @@
|
|||
<inertia
|
||||
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
|
||||
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
|
||||
izz="${trunk_izz}"/>
|
||||
izz="${trunk_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,137 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<ros2_control name="GazeboSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="FR_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_hip_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${hip_torque_max}</param>
|
||||
<param name="max">${hip_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_thigh_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${thigh_torque_max}</param>
|
||||
<param name="max">${thigh_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FR_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="FL_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RR_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
<joint name="RL_calf_joint">
|
||||
<command_interface name="effort">
|
||||
<param name="min">-${calf_torque_max}</param>
|
||||
<param name="max">${calf_torque_max}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
||||
<parameters>$(find go2_description)/config/robot_control_group.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
cmake_minimum_required(VERSION 3.0)
|
||||
|
||||
project(gr1t1_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)
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
gr1t1_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
|
||||
# left leg Controllers ---------------------------------------
|
||||
l_hip_roll_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_roll
|
||||
pid: {p: 57.0, i: 0.0, d: 5.7}
|
||||
|
||||
l_hip_yaw_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_yaw
|
||||
pid: {p: 43.0, i: 0.0, d: 4.3}
|
||||
|
||||
l_hip_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
l_knee_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_knee_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
l_ankle_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_ankle_pitch
|
||||
pid: {p: 15.3, i: 0.0, d: 1.5}
|
||||
|
||||
# right leg Controllers ---------------------------------------
|
||||
r_hip_roll_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_roll
|
||||
pid: {p: 57.0, i: 0.0, d: 5.7}
|
||||
|
||||
r_hip_yaw_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_yaw
|
||||
pid: {p: 43.0, i: 0.0, d: 4.3}
|
||||
|
||||
r_hip_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
r_knee_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_knee_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
r_ankle_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_ankle_pitch
|
||||
pid: {p: 15.3, i: 0.0, d: 1.5}
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<launch>
|
||||
<arg
|
||||
name="model" />
|
||||
<param
|
||||
name="robot_description"
|
||||
textfile="$(find gr1t1_description)/urdf/GR1T1_lower_limb.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 gr1t1_description)/urdf.rviz" />
|
||||
</launch>
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<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 $(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf -urdf -model gr1t1_description"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,20 +0,0 @@
|
|||
<package format="2">
|
||||
<name>gr1t1_description</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
<p>URDF Description package for gr1t1_description</p>
|
||||
<p>This package contains configuration data, 3D models and launch files for fldlar_description robot</p>
|
||||
</description>
|
||||
<author>Ziqi Fan</author>
|
||||
<maintainer email="fanziqi614@gmail.com" />
|
||||
<license>Apache</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>
|
||||
|
|
@ -1,225 +0,0 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 549
|
||||
- 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
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
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:
|
||||
10:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
11:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
12:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
13:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
20:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
21:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
22:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
23:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
30:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
31:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
32:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
33:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
40:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
41:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
42:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
43:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
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
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
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
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- 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: 2.634476661682129
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0.0988716408610344
|
||||
Y: 0.08353396505117416
|
||||
Z: -0.19447802007198334
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.5753985047340393
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 0.47539806365966797
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 278
|
||||
Y: 96
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -1,14 +0,0 @@
|
|||
cmake_minimum_required(VERSION 3.0)
|
||||
|
||||
project(gr1t2_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)
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
gr1t2_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
|
||||
# left leg Controllers ---------------------------------------
|
||||
l_hip_roll_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_roll
|
||||
pid: {p: 57.0, i: 0.0, d: 5.7}
|
||||
|
||||
l_hip_yaw_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_yaw
|
||||
pid: {p: 43.0, i: 0.0, d: 4.3}
|
||||
|
||||
l_hip_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_hip_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
l_knee_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_knee_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
l_ankle_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: l_ankle_pitch
|
||||
pid: {p: 15.3, i: 0.0, d: 1.5}
|
||||
|
||||
# right leg Controllers ---------------------------------------
|
||||
r_hip_roll_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_roll
|
||||
pid: {p: 57.0, i: 0.0, d: 5.7}
|
||||
|
||||
r_hip_yaw_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_yaw
|
||||
pid: {p: 43.0, i: 0.0, d: 4.3}
|
||||
|
||||
r_hip_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_hip_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
r_knee_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_knee_pitch
|
||||
pid: {p: 114.0, i: 0.0, d: 11.4}
|
||||
|
||||
r_ankle_pitch_controller:
|
||||
type: robot_joint_controller/RobotJointController
|
||||
joint: r_ankle_pitch
|
||||
pid: {p: 15.3, i: 0.0, d: 1.5}
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<launch>
|
||||
<arg
|
||||
name="model" />
|
||||
<param
|
||||
name="robot_description"
|
||||
textfile="$(find gr1t2_description)/urdf/GR1T2_simple.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 gr1t2_description)/urdf.rviz" />
|
||||
</launch>
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<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 $(find gr1t2_description)/urdf/GR1T2_simple.urdf -urdf -model gr1t2_description"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue