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

run ocs2 on gazebo successfully

This commit is contained in:
Huang Zhenbiao 2025-02-26 12:48:19 +08:00
parent d8c4d8eee5
commit 5139201922
7 changed files with 39 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
controllers/ocs2_quadruped_controller/README.md
View File

@ -85,4 +85,10 @@ source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=go2_description
```
### 3.2 Gazebo Launch
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=go2_description
```
At the first launch, controller may compile the OCS2 model and generate the shared library. The compilation process may take a few minutes. After the compilation, restart the controller and the robot should stand up. Then you can use the keyboard or joystick to control the robot (Keyboard 2 or Joystick LB+A to Trot mode).

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

@ -14,7 +14,7 @@ Panels:
- /Target Trajectories1/Target Feet Trajectories1/Marker4
- /Target Trajectories1/Target Base Trajectory1
Splitter Ratio: 0.5
Tree Height: 372
Tree Height: 531
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@ -46,7 +46,7 @@ Visualization Manager:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 0; 170; 255
Color: 0; 0; 0
Enabled: true
Line Style:
Line Width: 0.029999999329447746
@ -400,7 +400,7 @@ Visualization Manager:
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Background Color: 198; 198; 198
Fixed Frame: odom
Frame Rate: 30
Name: root
@ -442,33 +442,33 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 4.046566963195801
Distance: 3.2034783363342285
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.5368534326553345
Y: 0.8848452568054199
Z: 0.04236998409032822
X: 0.21563361585140228
Y: 0.6374001502990723
Z: -0.11019232869148254
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4303978383541107
Pitch: 0.41039788722991943
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 5.236791610717773
Yaw: 5.176790714263916
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 655
Height: 825
Hide Left Dock: true
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001d500000239fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000239000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c700ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000023900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001d5000002d8fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000002d8000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000c700ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004af000002e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -477,6 +477,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1024
X: 764
Y: 148
Width: 1199
X: 1337
Y: 153

41
controllers/ocs2_quadruped_controller/launch/gazebo.launch.py
View File

@ -4,7 +4,7 @@ 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.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
@ -18,7 +18,6 @@ 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']
@ -51,7 +50,7 @@ def launch_setup(context, *args, **kwargs):
],
)
rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "rviz.rviz")
rviz_config_file = os.path.join(get_package_share_directory("ocs2_quadruped_controller"), "config", "visualize_ocs2.rviz")
rviz = Node(
package='rviz2',
executable='rviz2',
@ -74,13 +73,6 @@ def launch_setup(context, *args, **kwargs):
"--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",
@ -97,17 +89,20 @@ def launch_setup(context, *args, **kwargs):
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 ', default_sdf_path])]),
leg_pd_controller,
imu_sensor_broadcaster,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher],
target_action=imu_sensor_broadcaster,
on_exit=[joint_state_publisher],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_publisher,
on_exit=[ocs2_controller],
on_exit=[TimerAction(
period=10.0, # 延迟5秒启动
actions=[ocs2_controller]
)],
)
),
]
@ -132,32 +127,22 @@ def generate_launch_description():
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"
# "/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},
]
output="screen"
)
return LaunchDescription([
world,
pkg_description,
height,
controller,
gz_bridge_node,
OpaqueFunction(function=launch_setup),
])

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

@ -37,7 +37,7 @@ namespace ocs2::legged_robot {
void StateEstimateBase::updateContact() {
const size_t size = ctrl_component_.foot_force_state_interface_.size();
for (int i = 0; i < size; i++) {
contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 0.1;
contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 10.0;
}
}

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

@ -26,7 +26,7 @@ imu_sensor_broadcaster:
ocs2_quadruped_controller:
ros__parameters:
update_rate: 100 # Hz
update_rate: 200 # Hz
robot_pkg: "go2_description"
joints:
- FL_hip_joint

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

@ -254,10 +254,10 @@
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="FL"/>
<xacro:ft_sensor name="RR"/>
<xacro:ft_sensor name="RL"/>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="RR"/>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">

3
hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp
View File

@ -336,7 +336,8 @@ namespace gz_quadruped_hardware {
this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns);
this->dataPtr->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
this->dataPtr->executor_->add_node(this->dataPtr->node_);
auto spin = [this]() {
auto spin = [this]
{
this->dataPtr->executor_->spin();
};
this->dataPtr->thread_executor_spin_ = std::thread(spin);