EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix leg controller in jazzy, simplified gazebo launch file

This commit is contained in:
Zhenbiao Huang 2024-10-20 12:42:34 +08:00
parent d3f30e7750
commit 4575779eb5
20 changed files with 86 additions and 927 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
controllers/leg_pd_controller/CMakeLists.txt
View File

@ -21,6 +21,15 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach ()
# check ros2 control version
find_package(controller_manager 3.0.0)
# Handle the case where the required version is not found
if(NOT controller_manager_FOUND)
add_definitions(-DROS2_CONTROL_VERSION_LT_3)
message(FATAL_ERROR "ros2_control version below 3.0.0. "
"Change the implementation to support ros2_control version 2.")
endif()
add_library(${PROJECT_NAME} SHARED
src/LegPdController.cpp
)

10
controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h
View File

@ -37,10 +37,14 @@ namespace leg_pd_controller {
protected:
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
// controller_interface::return_type update_reference_from_subscribers(
// const rclcpp::Time &time, const rclcpp::Duration &period);
#ifdef ROS2_CONTROL_VERSION_LT_3
controller_interface::return_type update_reference_from_subscribers() override;
#else
controller_interface::return_type update_reference_from_subscribers(
const rclcpp::Time &time, const rclcpp::Duration &period);
#endif
std::vector<double> joint_effort_command_;
std::vector<double> joint_position_command_;

102
controllers/leg_pd_controller/src/LegPdController.cpp
View File

@ -4,22 +4,17 @@
#include "leg_pd_controller/LegPdController.h"
namespace leg_pd_controller
{
namespace leg_pd_controller {
using config_type = controller_interface::interface_configuration_type;
controller_interface::CallbackReturn LegPdController::on_init()
{
try
{
joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
controller_interface::CallbackReturn LegPdController::on_init() {
try {
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
reference_interface_types_ =
auto_declare<std::vector<std::string>>("reference_interfaces", reference_interface_types_);
auto_declare<std::vector<std::string> >("reference_interfaces", reference_interface_types_);
state_interface_types_ = auto_declare<std::vector<
std::string>>("state_interfaces", state_interface_types_);
}
catch (const std::exception& e)
{
std::string> >("state_interfaces", state_interface_types_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
}
@ -34,27 +29,22 @@ namespace leg_pd_controller
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size());
for (const auto& joint_name : joint_names_)
{
for (const auto &joint_name: joint_names_) {
conf.names.push_back(joint_name + "/effort");
}
return conf;
}
controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const
{
controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto& joint_name : joint_names_)
{
for (const auto& interface_type : state_interface_types_)
{
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: state_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
@ -62,28 +52,26 @@ namespace leg_pd_controller
}
controller_interface::CallbackReturn LegPdController::on_configure(
const rclcpp_lifecycle::State& /*previous_state*/)
{
reference_interfaces_.resize(joint_names_.size()*5, std::numeric_limits<double>::quiet_NaN());
const rclcpp_lifecycle::State & /*previous_state*/) {
#ifdef ROS2_CONTROL_VERSION_LT_3
reference_interfaces_.resize(joint_names_.size() * 5, std::numeric_limits<double>::quiet_NaN());
#endif
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn LegPdController::on_activate(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
joint_effort_command_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
// assign effort command interface
for (auto& interface : command_interfaces_)
{
for (auto &interface: command_interfaces_) {
joint_effort_command_interface_.emplace_back(interface);
}
// assign state interfaces
for (auto& interface : state_interfaces_)
{
for (auto &interface: state_interfaces_) {
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
@ -91,64 +79,57 @@ namespace leg_pd_controller
}
controller_interface::CallbackReturn LegPdController::on_deactivate(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
release_interfaces();
return CallbackReturn::SUCCESS;
}
bool LegPdController::on_set_chained_mode(bool /*chained_mode*/)
{
bool LegPdController::on_set_chained_mode(bool /*chained_mode*/) {
return true;
}
controller_interface::return_type LegPdController::update_and_write_commands(
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
if (joint_names_.size() != joint_effort_command_.size() ||
joint_names_.size() != joint_kp_command_.size() ||
joint_names_.size() != joint_position_command_.size() ||
joint_names_.size() != joint_position_state_interface_.size() ||
joint_names_.size() != joint_velocity_state_interface_.size() ||
joint_names_.size() != joint_effort_command_interface_.size())
{
joint_names_.size() != joint_effort_command_interface_.size()) {
std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl;
std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() <<
std::endl;
std::endl;
std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() <<
std::endl;
std::endl;
std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() <<
std::endl;
std::endl;
throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
}
for (size_t i = 0; i < joint_names_.size(); ++i)
{
for (size_t i = 0; i < joint_names_.size(); ++i) {
// PD Controller
const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
joint_position_command_[i] - joint_position_state_interface_[i].get().get_value())
+
joint_kd_command_[i] * (
joint_velocities_command_[i] - joint_velocity_state_interface_[i].get().
get_value());
joint_position_command_[i] - joint_position_state_interface_[i].get().get_value())
+
joint_kd_command_[i] * (
joint_velocities_command_[i] - joint_velocity_state_interface_[i].get().
get_value());
joint_effort_command_interface_[i].get().set_value(torque);
}
return controller_interface::return_type::OK;
}
std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces()
{
std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces() {
std::vector<hardware_interface::CommandInterface> reference_interfaces;
int ind = 0;
std::string controller_name = get_node()->get_name();
for (const auto& joint_name : joint_names_)
{
for (const auto &joint_name: joint_names_) {
std::cout << joint_name << std::endl;
reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
@ -162,15 +143,16 @@ namespace leg_pd_controller
return reference_interfaces;
}
// controller_interface::return_type LegPdController::update_reference_from_subscribers(
// const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
// return controller_interface::return_type::OK;
// }
controller_interface::return_type LegPdController::update_reference_from_subscribers()
{
#ifdef ROS2_CONTROL_VERSION_LT_3
controller_interface::return_type LegPdController::update_reference_from_subscribers() {
return controller_interface::return_type::OK;
}
#else
controller_interface::return_type LegPdController::update_reference_from_subscribers(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
return controller_interface::return_type::OK;
}
#endif
}
#include <pluginlib/class_list_macros.hpp>

17
controllers/unitree_guide_controller/launch/gazebo.launch.py
View File

@ -10,13 +10,10 @@ from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
package_description = "go2_description"
def launch_setup(context, *args, **kwargs):
package_description = context.launch_configurations['pkg_description']
init_height = context.launch_configurations['height']
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
@ -37,7 +34,7 @@ def launch_setup(context, *args, **kwargs):
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', '0.5'],
'robot', '-allow_renaming', 'true', '-z', init_height],
)
robot_state_publisher = Node(
@ -83,7 +80,6 @@ def launch_setup(context, *args, **kwargs):
return [
rviz,
robot_state_publisher,
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
@ -115,7 +111,14 @@ def generate_launch_description():
description='package for robot description'
)
height = DeclareLaunchArgument(
'height',
default_value='0.5',
description='Init height in simulation'
)
return LaunchDescription([
pkg_description,
height,
OpaqueFunction(function=launch_setup),
])
])

2
descriptions/deep_robotics/lite3_description/README.md
View File

@ -43,7 +43,7 @@ ros2 launch lite3_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch lite3_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43
```
* Legged Gym Controller
```bash

106
descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "lite3_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'lite3', '-allow_renaming', 'true', '-z', '0.4'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

2
descriptions/deep_robotics/x30_description/README.md
View File

@ -39,7 +39,7 @@ ros2 launch x30_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch x30_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=x30_description height:=0.64
```
* Legged Gym Controller
```bash

106
descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "x30_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'x30', '-allow_renaming', 'true', '-z', '0.4'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

11
descriptions/unitree/a1_description/README.md
View File

@ -1,27 +1,34 @@
# Unitree A1 Description
This repository contains the urdf model of A1.
![A1](../../../.images/a1.png)
Tested environment:
* Ubuntu 24.04
* ROS2 Jazzy
## Build
```bash
cd ~/ros2_ws
colcon build --packages-up-to a1_description --symlink-install
```
## Visualize the robot
To visualize and check the configuration of the robot in rviz, simply launch:
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description visualize.launch.py
```
## Launch ROS2 Control
### Mujoco Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
@ -39,6 +46,7 @@ ros2 launch a1_description visualize.launch.py
```
### Gazebo Classic 11 (ROS2 Humble)
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
@ -46,10 +54,11 @@ ros2 launch a1_description visualize.launch.py
```
### Gazebo Harmonic (ROS2 Jazzy)
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=a1_description height:=0.43
```
* RL Quadruped Controller
```bash

2
descriptions/unitree/a1_description/config/gazebo.yaml
View File

@ -1,7 +1,7 @@
# Controller Manager configuration
controller_manager:
ros__parameters:
update_rate: 2000 # Hz
update_rate: 1000 # Hz
# Define the available controllers
joint_state_broadcaster:

106
descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "a1_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'a1', '-allow_renaming', 'true', '-z', '0.4'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

2
descriptions/unitree/aliengo_description/README.md
View File

@ -45,5 +45,5 @@ ros2 launch aliengo_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch aliengo_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=aliengo_description height:=0.535
```

106
descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "aliengo_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', '0.5'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

4
descriptions/unitree/b2_description/README.md
View File

@ -40,5 +40,5 @@ ros2 launch b2_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch b2_description gazebo.launch.py
```
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=b2_description height:=0.68
```

106
descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "b2_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', '0.68'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

2
descriptions/unitree/go1_description/README.md
View File

@ -40,5 +40,5 @@ ros2 launch go1_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch go1_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=go1_description
```

106
descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "go1_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'go2', '-allow_renaming', 'true', '-z', '0.5'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

106
descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "go2_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', '0.5'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

2
descriptions/xiaomi/cyberdog_description/README.md
View File

@ -41,5 +41,5 @@ ros2 launch cyberdog_description visualize.launch.py
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch cyberdog_description gazebo.launch.py
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=cyberdog_description height:=0.31
```

106
descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py
View File

@ -1,106 +0,0 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "cyberdog_description"
def process_xacro():
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, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', '0.46'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])