run ocs2 on gazebo successfully

controllers/ocs2_quadruped_controller/README.md

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

controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz

@@ -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
+        X: 0.21563361585140228
-        Y: 0.8848452568054199
+        Y: 0.6374001502990723
-        Z: 0.04236998409032822
+        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
+  Width: 1199
-  X: 764
+  X: 1337
-  Y: 148
+  Y: 153

controllers/ocs2_quadruped_controller/launch/gazebo.launch.py

@@ -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,
+                target_action=imu_sensor_broadcaster,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher],
+                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@nav_msgs/msg/Odometry@gz.msgs.Odometry",
-            "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
+            # "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
-            "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
+            # "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
         ],
-        output="screen",
+        output="screen"
-        parameters=[
-            {'use_sim_time': True},
-        ]
     )
 
     return LaunchDescription([
         world,
         pkg_description,
         height,
-        controller,
         gz_bridge_node,
         OpaqueFunction(function=launch_setup),
     ])

controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp

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

descriptions/unitree/go2_description/config/gazebo.yaml

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

descriptions/unitree/go2_description/xacro/gazebo.xacro

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

hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp

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