From 69e9e1f47764cd260505a3febab4f4d6409abcc7 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Sun, 23 Feb 2025 22:07:46 +0800 Subject: [PATCH] add gazebo ocs2 simulation --- .../ocs2_quadruped_controller/CMakeLists.txt | 2 + .../ocs2_quadruped_controller/README.md | 21 +- .../config/visualize_ocs2.rviz | 155 ++----------- .../estimator/FromOdomTopic.h | 23 ++ .../estimator/StateEstimateBase.h | 2 + .../launch/gazebo.launch.py | 157 +++++++++++++ .../launch/mujoco.launch.py | 12 +- .../src/Ocs2QuadrupedController.cpp | 206 ++++++++++++------ .../src/Ocs2QuadrupedController.h | 9 +- .../src/control/GaitManager.cpp | 25 ++- .../src/control/TargetManager.cpp | 22 +- .../src/estimator/FromOdomTopic.cpp | 41 ++++ .../src/estimator/GroundTruth.cpp | 18 +- .../src/estimator/LinearKalmanFilter.cpp | 1 + .../src/estimator/StateEstimateBase.cpp | 9 +- .../go2_description/config/gazebo.yaml | 58 +++++ .../go2_description/config/robot_control.yaml | 4 +- .../go2_description/xacro/gazebo.xacro | 11 + libraries/gz_quadruped_playground/README.md | 17 +- .../gz_quadruped_playground/config/rviz.rviz | 29 +-- .../launch/gazebo.launch.py | 5 +- .../launch/ocs2.launch.py | 44 ++++ .../launch/unitree_guide.launch.py | 21 +- libraries/gz_quadruped_playground/package.xml | 1 + 24 files changed, 601 insertions(+), 292 deletions(-) create mode 100644 controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h create mode 100644 controllers/ocs2_quadruped_controller/launch/gazebo.launch.py create mode 100644 controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp create mode 100644 libraries/gz_quadruped_playground/launch/ocs2.launch.py diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index c62569d..82b0a21 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -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 diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 3d20ac0..70db7cc 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -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 diff --git a/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz b/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz index 28e17c2..e052dbc 100644 --- a/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz +++ b/controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz @@ -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 diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h new file mode 100644 index 0000000..d88b3fc --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h @@ -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::SharedPtr odom_sub_; + vector3_t position_; + vector3_t linear_velocity_; + }; +}; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index 6acb2c6..e451755 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -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); diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py new file mode 100644 index 0000000..004dbbc --- /dev/null +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -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), + ]) diff --git a/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py b/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py index fb8f9a5..c85bc97 100644 --- a/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py @@ -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"), ], diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 2941e71..4f16f48 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -3,6 +3,7 @@ // #include "Ocs2QuadrupedController.h" +#include #include #include @@ -16,43 +17,59 @@ #include #include #include +#include -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_) { - conf.names.push_back(joint_name + "/" += interface_type); + 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("urdf_file", urdf_file_); - task_file_ = auto_declare("task_file", task_file_); - reference_file_ = auto_declare("reference_file", reference_file_); - gait_file_ = auto_declare("gait_file", gait_file_); + robot_pkg_ = auto_declare("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 >("joints", joint_names_); - feet_names_ = auto_declare >("feet", feet_names_); + command_prefix_ = auto_declare("command_prefix", command_prefix_); + joint_names_ = auto_declare>("joints", joint_names_); + feet_names_ = auto_declare>("feet", feet_names_); command_interface_types_ = - auto_declare >("command_interfaces", command_interface_types_); + auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = - auto_declare >("state_interfaces", state_interface_types_); + auto_declare>("state_interfaces", state_interface_types_); // IMU Sensor imu_name_ = auto_declare("imu_name", imu_name_); - imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); + imu_interface_types_ = auto_declare>("imu_interfaces", state_interface_types_); // Odometer Sensor (Ground Truth) - use_odom_ = auto_declare("use_odom", use_odom_); - if (use_odom_) { + estimator_type_ = auto_declare("estimator_type", estimator_type_); + if (estimator_type_ == "ground_truth") + { odom_name_ = auto_declare("odom_name", odom_name_); - odom_interface_types_ = auto_declare >("odom_interfaces", state_interface_types_); - } else { + odom_interface_types_ = auto_declare>("odom_interfaces", state_interface_types_); + } + else + { // Foot Force Sensor foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); foot_force_interface_types_ = - auto_declare >("foot_force_interfaces", state_interface_types_); + auto_declare>("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", 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(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(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(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(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), legged_interface_->getOptimalControlProblem(), legged_interface_->getInitializer()); @@ -311,7 +364,7 @@ namespace ocs2::legged_robot { // Initialize the reference manager const auto gait_manager_ptr = std::make_shared( ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()-> - getGaitSchedule()); + getGaitSchedule()); gait_manager_ptr->init(gait_file_); mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr()); @@ -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_); 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,30 +411,40 @@ 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(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(legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_, ctrl_comp_, get_node()); - dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); + dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator"); } + else + { + ctrl_comp_.estimator_ = std::make_shared(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_); ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance( - yaw_last, ctrl_comp_.observation_.state(9)); + yaw_last, ctrl_comp_.observation_.state(9)); // ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode(); } } diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index 168960e..50ec691 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -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 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 imu_interface_types_; @@ -94,7 +96,7 @@ namespace ocs2::legged_robot { std::vector foot_force_interface_types_; // Odometer Sensor (Ground Truth) - bool use_odom_ = false; + std::string estimator_type_ = "linear_kalman"; std::string odom_name_; std::vector odom_interface_types_; @@ -110,6 +112,7 @@ namespace ocs2::legged_robot { std::string gait_file_; bool verbose_; + bool mpc_need_updated_; std::shared_ptr legged_interface_; std::shared_ptr eeKinematicsPtr_; diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index 875d780..1ab2a68 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -8,19 +8,23 @@ #include -namespace ocs2::legged_robot { - GaitManager::GaitManager(CtrlComponent &ctrl_component, +namespace ocs2::legged_robot +{ + GaitManager::GaitManager(CtrlComponent& ctrl_component, std::shared_ptr 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 ¤tState, - 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; diff --git a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp index 845e116..9ca460d 100644 --- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp @@ -9,13 +9,15 @@ #include "ocs2_quadruped_controller/control/CtrlComponent.h" -namespace ocs2::legged_robot { - TargetManager::TargetManager(CtrlComponent &ctrl_component, - const std::shared_ptr &referenceManagerPtr, - const std::string &task_file, - const std::string &reference_file) +namespace ocs2::legged_robot +{ + TargetManager::TargetManager(CtrlComponent& ctrl_component, + const std::shared_ptr& 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 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_; @@ -49,7 +53,7 @@ namespace ocs2::legged_robot { const scalar_t targetReachingTime = ctrl_component_.observation_.time + time_to_target_; auto trajectories = - targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime); + targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime); trajectories.stateTrajectory[0].head(3) = cmd_vel_rot; trajectories.stateTrajectory[1].head(3) = cmd_vel_rot; diff --git a/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp b/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp new file mode 100644 index 0000000..7abf9f5 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp @@ -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( + "/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_; + } +} diff --git a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp index 968aa4d..f1b20d6 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp @@ -6,17 +6,20 @@ #include -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); diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index 6d606f2..3418341 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -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) { diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 6f0995c..d869562 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -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("odom", 10); - pose_pub_ = node_->create_publisher("pose", 10); + } void StateEstimateBase::updateJointStates() { @@ -72,6 +71,12 @@ namespace ocs2::legged_robot { updateAngular(zyx, angularVelGlobal); } + void StateEstimateBase::initPublishers() + { + odom_pub_ = node_->create_publisher("odom", 10); + pose_pub_ = node_->create_publisher("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; diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index 221922d..9fdcef3 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -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 diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 71538f8..61058c1 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -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 diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index efc7afc..d770678 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -110,6 +110,16 @@ + + odom + base + 200 + odom + 3 + odom_with_covariance + tf + @@ -185,6 +195,7 @@ true + diff --git a/libraries/gz_quadruped_playground/README.md b/libraries/gz_quadruped_playground/README.md index 93473a1..56e2150 100644 --- a/libraries/gz_quadruped_playground/README.md +++ b/libraries/gz_quadruped_playground/README.md @@ -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 -``` \ No newline at end of file +* 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) \ No newline at end of file diff --git a/libraries/gz_quadruped_playground/config/rviz.rviz b/libraries/gz_quadruped_playground/config/rviz.rviz index ab52823..cf6672b 100644 --- a/libraries/gz_quadruped_playground/config/rviz.rviz +++ b/libraries/gz_quadruped_playground/config/rviz.rviz @@ -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: + 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: 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 diff --git a/libraries/gz_quadruped_playground/launch/gazebo.launch.py b/libraries/gz_quadruped_playground/launch/gazebo.launch.py index 596aa63..5204690 100644 --- a/libraries/gz_quadruped_playground/launch/gazebo.launch.py +++ b/libraries/gz_quadruped_playground/launch/gazebo.launch.py @@ -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=[ diff --git a/libraries/gz_quadruped_playground/launch/ocs2.launch.py b/libraries/gz_quadruped_playground/launch/ocs2.launch.py new file mode 100644 index 0000000..c4c231e --- /dev/null +++ b/libraries/gz_quadruped_playground/launch/ocs2.launch.py @@ -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], + ) + ), + ]) diff --git a/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py index 0f7f718..29b972d 100644 --- a/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py +++ b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py @@ -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([ diff --git a/libraries/gz_quadruped_playground/package.xml b/libraries/gz_quadruped_playground/package.xml index 46f27b8..2a14eb4 100644 --- a/libraries/gz_quadruped_playground/package.xml +++ b/libraries/gz_quadruped_playground/package.xml @@ -10,6 +10,7 @@ xacro joint_state_publisher robot_state_publisher + leg_pd_controller imu_sensor_broadcaster