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

add gazebo ocs2 simulation

This commit is contained in:
Huang Zhenbiao 2025-02-23 22:07:46 +08:00
parent 554b7dbc6a
commit 69e9e1f477
24 changed files with 601 additions and 292 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -19,6 +19,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
angles
nav_msgs
qpoases_colcon
ament_index_cpp
)
# find dependencies
@ -33,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED
src/estimator/GroundTruth.cpp
src/estimator/LinearKalmanFilter.cpp
src/estimator/StateEstimateBase.cpp
src/estimator/FromOdomTopic.cpp
src/wbc/HierarchicalWbc.cpp
src/wbc/HoQp.cpp

21
controllers/ocs2_quadruped_controller/README.md
View File

@ -38,19 +38,26 @@ Required hardware interfaces:
## 2. Build
### 2.1 Build Dependencies
Before install OCS2 ROS2, please follow the guide to install [Pinocchio](https://stack-of-tasks.github.io/pinocchio/download.html), don't use the pinocchio install by rosdep!
Before install OCS2 ROS2, please follow the guide to install [Pinocchio](https://stack-of-tasks.github.io/pinocchio/download.html). **Don't use the pinocchio install by rosdep**!
* OCS2 ROS2 Libraries
```bash
colcon build --packages-up-to ocs2_legged_robot_ros
colcon build --packages-up-to ocs2_self_collision
```
**After installed Pinocchio**, follow below step to clone ocs2 ros2 library to src folder.
```bash
cd ~/ros2_ws/src
git clone https://github.com/legubiao/ocs2_ros2
cd ocs2_ros2
git submodule update --init --recursive
cd ..
rosdep install --from-paths src --ignore-src -r -y
```
### 2.2 Build OCS2 Quadruped Controller
```bash
cd ~/ros2_ws
colcon build --packages-up-to ocs2_quadruped_controller
colcon build --packages-up-to ocs2_quadruped_controller --symlink-install
```
## 3. Launch

155
controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz
View File

@ -4,15 +4,17 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Grid1
- /RobotModel1
- /TF1/Frames1
- /Optimized State Trajectory1
- /Target Trajectories1/Target Feet Trajectories1/Marker1
- /Target Trajectories1/Target Feet Trajectories1/Marker2
- /Target Trajectories1/Target Feet Trajectories1/Marker3
- /Target Trajectories1/Target Feet Trajectories1/Marker4
- /Target Trajectories1/Target Base Trajectory1
Splitter Ratio: 0.5
Tree Height: 477
Tree Height: 372
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@ -37,14 +39,14 @@ Panels:
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.8294117450714111
Tree Height: 301
Tree Height: 305
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Color: 0; 170; 255
Enabled: true
Line Style:
Line Width: 0.029999999329447746
@ -56,10 +58,10 @@ Visualization Manager:
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Plane Cell Count: 100
Reference Frame: odom
Value: true
- Alpha: 0.4000000059604645
- Alpha: 0.800000011920929
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
@ -81,11 +83,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
FL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
@ -96,31 +93,16 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
FL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
@ -131,32 +113,17 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
FR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
@ -167,31 +134,16 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
RL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
@ -202,46 +154,25 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
RR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_Link:
front_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
lidar_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tail_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
@ -264,71 +195,43 @@ Visualization Manager:
All Enabled: false
FL_calf:
Value: true
FL_calf_rotor:
Value: true
FL_foot:
Value: true
FL_hip:
Value: true
FL_hip_rotor:
Value: true
FL_thigh:
Value: true
FL_thigh_rotor:
Value: true
FR_calf:
Value: true
FR_calf_rotor:
Value: true
FR_foot:
Value: true
FR_hip:
Value: true
FR_hip_rotor:
Value: true
FR_thigh:
Value: true
FR_thigh_rotor:
Value: true
RL_calf:
Value: true
RL_calf_rotor:
Value: true
RL_foot:
Value: true
RL_hip:
Value: true
RL_hip_rotor:
Value: true
RL_thigh:
Value: true
RL_thigh_rotor:
Value: true
RR_calf:
Value: true
RR_calf_rotor:
Value: true
RR_foot:
Value: true
RR_hip:
Value: true
RR_hip_rotor:
Value: true
RR_thigh:
Value: true
RR_thigh_rotor:
Value: true
base:
Value: false
head_Link:
front_camera:
Value: true
imu_link:
Value: false
lidar_link:
Value: true
odom:
Value: false
tail_link:
Value: true
trunk:
Value: true
@ -346,53 +249,25 @@ Visualization Manager:
FL_calf:
FL_foot:
{}
FL_calf_rotor:
{}
FL_thigh_rotor:
{}
FL_hip_rotor:
{}
FR_hip:
FR_thigh:
FR_calf:
FR_foot:
{}
FR_calf_rotor:
{}
FR_thigh_rotor:
{}
FR_hip_rotor:
{}
RL_hip:
RL_thigh:
RL_calf:
RL_foot:
{}
RL_calf_rotor:
{}
RL_thigh_rotor:
{}
RL_hip_rotor:
{}
RR_hip:
RR_thigh:
RR_calf:
RR_foot:
{}
RR_calf_rotor:
{}
RR_thigh_rotor:
{}
RR_hip_rotor:
{}
head_Link:
front_camera:
{}
imu_link:
{}
lidar_link:
{}
tail_link:
{}
Update Interval: 0
Value: true
- Class: rviz_default_plugins/MarkerArray
@ -525,7 +400,7 @@ Visualization Manager:
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 238
Background Color: 0; 0; 0
Fixed Frame: odom
Frame Rate: 30
Name: root
@ -593,7 +468,7 @@ Window Geometry:
Height: 655
Hide Left Dock: true
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000032c000002a7fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005d00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005d00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003f000002a7000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000cc00ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000023100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001d500000239fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000239000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c700ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000023900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -603,5 +478,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1024
X: 828
Y: 212
X: 764
Y: 148

23
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h Normal file
View File

@ -0,0 +1,23 @@
//
// Created by biao on 25-2-23.
//
#pragma once
#include "StateEstimateBase.h"
namespace ocs2::legged_robot
{
class FromOdomTopic final : public StateEstimateBase
{
public:
FromOdomTopic(CentroidalModelInfo info, CtrlComponent& ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node);
vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
protected:
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
vector3_t position_;
vector3_t linear_velocity_;
};
};

2
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h
View File

@ -36,6 +36,8 @@ namespace ocs2::legged_robot {
size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); }
protected:
void initPublishers();
void updateAngular(const vector3_t &zyx, const vector_t &angularVel);
void updateLinear(const vector_t &pos, const vector_t &linearVel);

157
controllers/ocs2_quadruped_controller/launch/gazebo.launch.py Normal file
View File

@ -0,0 +1,157 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
import xacro
def launch_setup(context, *args, **kwargs):
# Gazebo World
world = context.launch_configurations['world']
default_sdf_path = os.path.join(get_package_share_directory('gz_quadruped_playground'), 'worlds', world + '.sdf')
print(default_sdf_path)
# Init Height When spawn the model
init_height = context.launch_configurations['height']
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'robot', '-allow_renaming', 'true', '-z', init_height],
)
# Robot Description
pkg_description = context.launch_configurations['pkg_description']
pkg_path = os.path.join(get_package_share_directory(pkg_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description = xacro.process_file(xacro_file, mappings={
'GAZEBO': 'true'
}).toxml()
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
}
],
)
rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "rviz.rviz")
rviz = Node(
package='rviz2',
executable='rviz2',
name='rviz',
output='screen',
arguments=["-d", rviz_config_file]
)
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"]
)
ocs2_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
)
return [
rviz,
robot_state_publisher,
gz_spawn_entity,
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 ', default_sdf_path])]),
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, ocs2_controller],
)
),
]
def generate_launch_description():
world = DeclareLaunchArgument(
'world',
default_value='default',
description='The world to load'
)
pkg_description = DeclareLaunchArgument(
'pkg_description',
default_value='go2_description',
description='package for robot description'
)
height = DeclareLaunchArgument(
'height',
default_value='0.5',
description='Init height in simulation'
)
controller = DeclareLaunchArgument(
'controller',
default_value='unitree_guide',
description='The ROS2-Control Controllers'
)
gz_bridge_node = Node(
package="ros_gz_bridge",
executable="parameter_bridge",
arguments=[
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
"/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
"/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
"/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
],
output="screen",
parameters=[
{'use_sim_time': True},
]
)
return LaunchDescription([
world,
pkg_description,
height,
controller,
gz_bridge_node,
OpaqueFunction(function=launch_setup),
])

12
controllers/ocs2_quadruped_controller/launch/mujoco.launch.py
View File

@ -53,17 +53,7 @@ def launch_setup(context, *args, **kwargs):
controller_manager = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers,
{
'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf',
'robot.urdf'),
'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
'task.info'),
'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'reference.info'),
'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
'ocs2', 'gait.info')
}],
parameters=[robot_controllers],
remappings=[
("~/robot_description", "/robot_description"),
],

200
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp
View File

@ -3,6 +3,7 @@
//
#include "Ocs2QuadrupedController.h"
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/thread_support/ExecuteAndSleep.h>
@ -16,43 +17,59 @@
#include <angles/angles.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
namespace ocs2::legged_robot {
namespace ocs2::legged_robot
{
using config_type = controller_interface::interface_configuration_type;
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const {
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: command_interface_types_) {
if (!command_prefix_.empty()) {
conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
} else {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
}
return conf;
}
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const {
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::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);
}
}
for (const auto &interface_type: imu_interface_types_) {
for (const auto& interface_type : imu_interface_types_)
{
conf.names.push_back(imu_name_ + "/" += interface_type);
}
if (use_odom_) {
for (const auto &interface_type: odom_interface_types_) {
if (estimator_type_ == "ground_truth")
{
for (const auto& interface_type : odom_interface_types_)
{
conf.names.push_back(odom_name_ + "/" += interface_type);
}
} else {
for (const auto &interface_type: foot_force_interface_types_) {
}
else if (estimator_type_ == "linear_kalman")
{
for (const auto& interface_type : foot_force_interface_types_)
{
conf.names.push_back(foot_force_name_ + "/" += interface_type);
}
}
@ -60,10 +77,11 @@ namespace ocs2::legged_robot {
return conf;
}
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
const rclcpp::Duration &period) {
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time& time,
const rclcpp::Duration& period)
{
// State Estimate
updateStateEstimation(time, period);
updateStateEstimation(period);
// Compute target trajectory
ctrl_comp_.target_manager_->update();
@ -73,6 +91,7 @@ namespace ocs2::legged_robot {
// Load the latest MPC policy
mpc_mrt_interface_->updatePolicy();
mpc_need_updated_ = true;
// Evaluate the current policy
vector_t optimized_state, optimized_input;
@ -96,9 +115,11 @@ namespace ocs2::legged_robot {
legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller
if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input)) {
if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input))
{
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
for (int i = 0; i < joint_names_.size(); i++) {
for (int i = 0; i < joint_names_.size(); i++)
{
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
@ -108,7 +129,8 @@ namespace ocs2::legged_robot {
return controller_interface::return_type::ERROR;
}
for (int i = 0; i < joint_names_.size(); i++) {
for (int i = 0; i < joint_names_.size(); i++)
{
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque(i));
ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
@ -125,12 +147,16 @@ namespace ocs2::legged_robot {
return controller_interface::return_type::OK;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init()
{
// Initialize OCS2
urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
task_file_ = auto_declare<std::string>("task_file", task_file_);
reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
robot_pkg_ = auto_declare<std::string>("robot_pkg", robot_pkg_);
const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_);
urdf_file_ = package_share_directory + "/urdf/robot.urdf";
task_file_ = package_share_directory + "/config/ocs2/task.info";
reference_file_ = package_share_directory + "/config/ocs2/reference.info";
gait_file_ = package_share_directory + "/config/ocs2/gait.info";
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
@ -140,27 +166,31 @@ namespace ocs2::legged_robot {
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
// Hardware Parameters
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string> >("feet", feet_names_);
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string>>("feet", feet_names_);
command_interface_types_ =
auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
// IMU Sensor
imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
imu_interface_types_ = auto_declare<std::vector<std::string>>("imu_interfaces", state_interface_types_);
// Odometer Sensor (Ground Truth)
use_odom_ = auto_declare<bool>("use_odom", use_odom_);
if (use_odom_) {
estimator_type_ = auto_declare<std::string>("estimator_type", estimator_type_);
if (estimator_type_ == "ground_truth")
{
odom_name_ = auto_declare<std::string>("odom_name", odom_name_);
odom_interface_types_ = auto_declare<std::vector<std::string> >("odom_interfaces", state_interface_types_);
} else {
odom_interface_types_ = auto_declare<std::vector<std::string>>("odom_interfaces", state_interface_types_);
}
else
{
// Foot Force Sensor
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
auto_declare<std::vector<std::string>>("foot_force_interfaces", state_interface_types_);
}
// PD gains
@ -200,9 +230,11 @@ namespace ocs2::legged_robot {
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg)
{
// Handle message
ctrl_comp_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.lx = msg->lx;
@ -218,38 +250,52 @@ namespace ocs2::legged_robot {
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
// clear out vectors in case of restart
ctrl_comp_.clear();
// assign command interfaces
for (auto &interface: command_interfaces_) {
for (auto& interface : command_interfaces_)
{
std::string interface_name = interface.get_interface_name();
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
if (const size_t pos = interface_name.find('/'); pos != std::string::npos)
{
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else {
}
else
{
command_interface_map_[interface_name]->push_back(interface);
}
}
// assign state interfaces
for (auto &interface: state_interfaces_) {
if (interface.get_prefix_name() == imu_name_) {
for (auto& interface : state_interfaces_)
{
if (interface.get_prefix_name() == imu_name_)
{
ctrl_comp_.imu_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == foot_force_name_) {
}
else if (interface.get_prefix_name() == foot_force_name_)
{
ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == odom_name_) {
}
else if (interface.get_prefix_name() == odom_name_)
{
ctrl_comp_.odom_state_interface_.emplace_back(interface);
} else {
}
else
{
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
}
if (mpc_running_ == false) {
if (mpc_running_ == false)
{
// Initial state
ctrl_comp_.observation_.state.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000));
updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000));
ctrl_comp_.observation_.input.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
ctrl_comp_.observation_.mode = STANCE;
@ -262,7 +308,8 @@ namespace ocs2::legged_robot {
mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
while (!mpc_mrt_interface_->initialPolicyReceived()) {
while (!mpc_mrt_interface_->initialPolicyReceived())
{
mpc_mrt_interface_->advanceMpc();
rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
}
@ -275,33 +322,39 @@ namespace ocs2::legged_robot {
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
release_interfaces();
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
const rclcpp_lifecycle::State & /*previous_state*/) {
const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS;
}
void Ocs2QuadrupedController::setupLeggedInterface() {
void Ocs2QuadrupedController::setupLeggedInterface()
{
legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
legged_interface_->setupJointNames(joint_names_, feet_names_);
legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
}
void Ocs2QuadrupedController::setupMpc() {
void Ocs2QuadrupedController::setupMpc()
{
mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
legged_interface_->getOptimalControlProblem(),
legged_interface_->getInitializer());
@ -321,25 +374,34 @@ namespace ocs2::legged_robot {
task_file_, reference_file_);
}
void Ocs2QuadrupedController::setupMrt() {
void Ocs2QuadrupedController::setupMrt()
{
mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
mpc_timer_.reset();
controller_running_ = true;
mpc_thread_ = std::thread([&] {
while (controller_running_) {
try {
mpc_thread_ = std::thread([&]
{
while (controller_running_)
{
try
{
executeAndSleep(
[&] {
if (mpc_running_) {
[&]
{
if (mpc_running_ && mpc_need_updated_)
{
mpc_need_updated_ = false;
mpc_timer_.startTimer();
mpc_mrt_interface_->advanceMpc();
mpc_timer_.endTimer();
}
},
legged_interface_->mpcSettings().mpcDesiredFrequency_);
} catch (const std::exception &e) {
}
catch (const std::exception& e)
{
controller_running_ = false;
RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
}
@ -349,25 +411,35 @@ namespace ocs2::legged_robot {
RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started.");
}
void Ocs2QuadrupedController::setupStateEstimate() {
if (use_odom_) {
void Ocs2QuadrupedController::setupStateEstimate()
{
if (estimator_type_ == "ground_truth")
{
ctrl_comp_.estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
ctrl_comp_,
get_node());
RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator");
} else {
}
else if (estimator_type_ == "linear_kalman")
{
ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_comp_,
get_node());
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
dynamic_cast<KalmanFilterEstimate&>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator");
}
else
{
ctrl_comp_.estimator_ = std::make_shared<FromOdomTopic>(legged_interface_->getCentroidalModelInfo(),ctrl_comp_,get_node());
RCLCPP_INFO(get_node()->get_logger(), "Using Odom Topic Based Estimator");
}
ctrl_comp_.observation_.time = 0;
}
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
measured_rbd_state_ = ctrl_comp_.estimator_->update(time, period);
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Duration& period)
{
measured_rbd_state_ = ctrl_comp_.estimator_->update(get_node()->now(), period);
ctrl_comp_.observation_.time += period.seconds();
const scalar_t yaw_last = ctrl_comp_.observation_.state(9);
ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);

9
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h
View File

@ -59,8 +59,7 @@ namespace ocs2::legged_robot {
void setupStateEstimate();
void updateStateEstimation(const rclcpp::Time &time,
const rclcpp::Duration &period);
void updateStateEstimation(const rclcpp::Duration &period);
CtrlComponent ctrl_comp_;
std::vector<std::string> joint_names_;
@ -85,6 +84,9 @@ namespace ocs2::legged_robot {
{"velocity", &ctrl_comp_.joint_velocity_state_interface_}
};
std::string robot_pkg_;
std::string command_prefix_;
// IMU Sensor
std::string imu_name_;
std::vector<std::string> imu_interface_types_;
@ -94,7 +96,7 @@ namespace ocs2::legged_robot {
std::vector<std::string> foot_force_interface_types_;
// Odometer Sensor (Ground Truth)
bool use_odom_ = false;
std::string estimator_type_ = "linear_kalman";
std::string odom_name_;
std::vector<std::string> odom_interface_types_;
@ -110,6 +112,7 @@ namespace ocs2::legged_robot {
std::string gait_file_;
bool verbose_;
bool mpc_need_updated_;
std::shared_ptr<LeggedInterface> legged_interface_;
std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;

25
controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp
View File

@ -8,19 +8,23 @@
#include <ocs2_core/misc/LoadData.h>
namespace ocs2::legged_robot {
GaitManager::GaitManager(CtrlComponent &ctrl_component,
namespace ocs2::legged_robot
{
GaitManager::GaitManager(CtrlComponent& ctrl_component,
std::shared_ptr<GaitSchedule> gait_schedule_ptr)
: ctrl_component_(ctrl_component),
gait_schedule_ptr_(std::move(gait_schedule_ptr)),
target_gait_({0.0, 1.0}, {STANCE}) {
target_gait_({0.0, 1.0}, {STANCE})
{
}
void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime,
const vector_t &currentState,
const ReferenceManagerInterface &referenceManager) {
const vector_t& currentState,
const ReferenceManagerInterface& referenceManager)
{
getTargetGait();
if (gait_updated_) {
if (gait_updated_)
{
const auto timeHorizon = finalTime - initTime;
gait_schedule_ptr_->insertModeSequenceTemplate(target_gait_, finalTime,
timeHorizon);
@ -28,19 +32,22 @@ namespace ocs2::legged_robot {
}
}
void GaitManager::init(const std::string &gait_file) {
void GaitManager::init(const std::string& gait_file)
{
gait_name_list_.clear();
loadData::loadStdVector(gait_file, "list", gait_name_list_, verbose_);
gait_list_.clear();
for (const auto &name: gait_name_list_) {
for (const auto& name : gait_name_list_)
{
gait_list_.push_back(loadModeSequenceTemplate(gait_file, name, verbose_));
}
RCLCPP_INFO(rclcpp::get_logger("gait_manager"), "GaitManager is ready.");
}
void GaitManager::getTargetGait() {
void GaitManager::getTargetGait()
{
if (ctrl_component_.control_inputs_.command == 0) return;
if (ctrl_component_.control_inputs_.command == last_command_) return;
last_command_ = ctrl_component_.control_inputs_.command;

20
controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
View File

@ -9,13 +9,15 @@
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot {
TargetManager::TargetManager(CtrlComponent &ctrl_component,
const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
const std::string &task_file,
const std::string &reference_file)
namespace ocs2::legged_robot
{
TargetManager::TargetManager(CtrlComponent& ctrl_component,
const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
const std::string& task_file,
const std::string& reference_file)
: ctrl_component_(ctrl_component),
referenceManagerPtr_(referenceManagerPtr) {
referenceManagerPtr_(referenceManagerPtr)
{
default_joint_state_ = vector_t::Zero(12);
loadData::loadCppDataType(reference_file, "comHeight", command_height_);
loadData::loadEigenMatrix(reference_file, "defaultJointState", default_joint_state_);
@ -24,7 +26,8 @@ namespace ocs2::legged_robot {
loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
}
void TargetManager::update() {
void TargetManager::update()
{
if (ctrl_component_.reset) return;
vector_t cmdGoal = vector_t::Zero(6);
cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
@ -36,7 +39,8 @@ namespace ocs2::legged_robot {
const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
vector_t cmd_vel_rot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
const vector_t targetPose = [&]() {
const vector_t targetPose = [&]()
{
vector_t target(6);
target(0) = currentPose(0) + cmd_vel_rot(0) * time_to_target_;
target(1) = currentPose(1) + cmd_vel_rot(1) * time_to_target_;

41
controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp Normal file
View File

@ -0,0 +1,41 @@
//
// Created by biao on 25-2-23.
//
#include "ocs2_quadruped_controller/estimator/FromOdomTopic.h"
namespace ocs2::legged_robot
{
FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent& ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node) :
StateEstimateBase(
std::move(info), ctrl_component,
node)
{
odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg)
{
// Handle message
position_ = {
msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z,
};
linear_velocity_ = {
msg->twist.twist.linear.x,
msg->twist.twist.linear.y,
msg->twist.twist.linear.z,
};
});
}
vector_t FromOdomTopic::update(const rclcpp::Time& time, const rclcpp::Duration& period)
{
updateJointStates();
updateImu();
updateLinear(position_, linear_velocity_);
return rbd_state_;
}
}

18
controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp
View File

@ -6,17 +6,20 @@
#include <ocs2_quadruped_controller/control/CtrlComponent.h>
namespace ocs2::legged_robot {
GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
namespace ocs2::legged_robot
{
GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent& ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node)
: StateEstimateBase(
std::move(info), ctrl_component,
node) {
node)
{
initPublishers();
}
vector_t GroundTruth::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
vector_t GroundTruth::update(const rclcpp::Time& time, const rclcpp::Duration& period)
{
updateJointStates();
updateContact();
updateImu();
position_ = {
@ -42,7 +45,8 @@ namespace ocs2::legged_robot {
return rbd_state_;
}
nav_msgs::msg::Odometry GroundTruth::getOdomMsg() {
nav_msgs::msg::Odometry GroundTruth::getOdomMsg()
{
nav_msgs::msg::Odometry odom;
odom.pose.pose.position.x = position_(0);
odom.pose.pose.position.y = position_(1);

1
controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp
View File

@ -49,6 +49,7 @@ namespace ocs2::legged_robot {
feet_heights_.setZero(numContacts_);
ee_kinematics_->setPinocchioInterface(pinocchio_interface_);
initPublishers();
}
vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) {

9
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp
View File

@ -18,8 +18,7 @@ namespace ocs2::legged_robot {
: ctrl_component_(ctrl_component),
info_(std::move(info)),
rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
}
void StateEstimateBase::updateJointStates() {
@ -72,6 +71,12 @@ namespace ocs2::legged_robot {
updateAngular(zyx, angularVelGlobal);
}
void StateEstimateBase::initPublishers()
{
odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
}
void StateEstimateBase::updateAngular(const vector3_t &zyx, const vector_t &angularVel) {
rbd_state_.segment<3>(0) = zyx;
rbd_state_.segment<3>(info_.generalizedCoordinatesNum) = angularVel;

58
descriptions/unitree/go2_description/config/gazebo.yaml
View File

@ -16,6 +16,9 @@ controller_manager:
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
ocs2_quadruped_controller:
type: ocs2_quadruped_controller/Ocs2QuadrupedController
rl_quadruped_controller:
type: rl_quadruped_controller/LeggedGymController
@ -48,6 +51,60 @@ leg_pd_controller:
- position
- velocity
ocs2_quadruped_controller:
ros__parameters:
update_rate: 100 # Hz
robot_pkg: "go2_description"
command_prefix: "leg_pd_controller"
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
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet:
- FL_foot
- FR_foot
- RL_foot
- RR_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
estimator_type: "odom_topic"
unitree_guide_controller:
ros__parameters:
update_rate: 200 # Hz
@ -103,6 +160,7 @@ rl_quadruped_controller:
ros__parameters:
update_rate: 200 # Hz
command_prefix: "leg_pd_controller"
robot_pkg: "go2_description"
joints:
- FL_hip_joint
- FL_thigh_joint

4
descriptions/unitree/go2_description/config/robot_control.yaml
View File

@ -77,7 +77,7 @@ unitree_guide_controller:
ocs2_quadruped_controller:
ros__parameters:
update_rate: 100 # Hz
robot_pkg: "go2_description"
joints:
- FL_hip_joint
- FL_thigh_joint
@ -125,7 +125,7 @@ ocs2_quadruped_controller:
- linear_acceleration.y
- linear_acceleration.z
use_odom: True
estimator_type: "ground_truth"
odom_name: "odometer"
odom_interfaces:
- position.x

11
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -110,6 +110,16 @@
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>200</odom_publish_frequency>
<odom_topic>odom</odom_topic>
<dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
<tf_topic>tf</tf_topic>
</plugin>
</gazebo>
<gazebo reference="trunk">
@ -185,6 +195,7 @@
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">
<sensor name="front_camera" type="camera">

17
libraries/gz_quadruped_playground/README.md
View File

@ -14,7 +14,16 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install
```
## Launch Simulation
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground gazebo.launch.py
```
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground gazebo.launch.py controller:=unitree_guide
```
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2
```
## Related Materials
* [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details)

29
libraries/gz_quadruped_playground/config/rviz.rviz
View File

@ -6,6 +6,7 @@ Panels:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 216
@ -31,9 +32,9 @@ Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Cell Size: 0.5
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Color: 0; 255; 255
Enabled: true
Line Style:
Line Width: 0.029999999329447746
@ -45,8 +46,8 @@ Visualization Manager:
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Plane Cell Count: 100
Reference Frame: odom
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
@ -189,8 +190,8 @@ Visualization Manager:
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base
Background Color: 0; 0; 0
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
@ -233,25 +234,25 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 0.9871627688407898
Distance: 2.8712024688720703
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.011728689074516296
Y: 0.011509218253195286
Z: -0.10348724573850632
X: -0.004370270296931267
Y: 0.06990259140729904
Z: -0.18453647196292877
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5053979158401489
Pitch: 0.310397744178772
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.8203961253166199
Yaw: 0.8653964400291443
Saved: ~
Window Geometry:
Displays:
@ -271,5 +272,5 @@ Window Geometry:
Views:
collapsed: true
Width: 1165
X: 1159
Y: 219
X: 1283
Y: 199

5
libraries/gz_quadruped_playground/launch/gazebo.launch.py
View File

@ -61,7 +61,7 @@ def launch_setup(context, *args, **kwargs):
)
# Controllers
controller = context.launch_configurations['world']
controller = context.launch_configurations['controller']
if controller == 'ocs2':
controller_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare('gz_quadruped_playground'),
@ -120,6 +120,9 @@ def generate_launch_description():
arguments=[
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
"/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
"/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
"/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
"/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
],
output="screen",
parameters=[

44
libraries/gz_quadruped_playground/launch/ocs2.launch.py Normal file
View File

@ -0,0 +1,44 @@
import xacro
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
def generate_launch_description():
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"]
)
ocs2_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
)
return LaunchDescription([
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, ocs2_controller],
)
),
])

21
libraries/gz_quadruped_playground/launch/unitree_guide.launch.py
View File

@ -3,44 +3,33 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
def generate_launch_description():
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
parameters=[
{'use_sim_time': True},
]
"--controller-manager", "/controller_manager"]
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
parameters=[
{'use_sim_time': True},
]
"--controller-manager", "/controller_manager"]
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
parameters=[
{'use_sim_time': True},
]
"--controller-manager", "/controller_manager"]
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
parameters=[
{'use_sim_time': True},
]
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"]
)
return LaunchDescription([

1
libraries/gz_quadruped_playground/package.xml
View File

@ -10,6 +10,7 @@
<exec_depend>xacro</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>leg_pd_controller</exec_depend>
<exec_depend>imu_sensor_broadcaster</exec_depend>
<export>