diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 70db7cc..26a6819 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/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). \ No newline at end of file diff --git a/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz b/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz index e052dbc..9803ee9 100644 --- a/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz +++ b/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 - 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: 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 diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index b18ffb9..c3958b2 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/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, - 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), ]) diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 3df6974..e0cc12b 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/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; } } diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index e0367fc..7465dbd 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/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 diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 911c9e2..24ecc3d 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -254,10 +254,10 @@ true - - + + diff --git a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp index 8c1ec99..68fae82 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp +++ b/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(); 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);