diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 750786b..8b337c6 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -81,7 +81,7 @@ install( ) install( - DIRECTORY config + DIRECTORY config launch DESTINATION share/${PROJECT_NAME}/ ) diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 014f641..12ceda8 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -4,8 +4,9 @@ This is a ros2-control controller based on [legged_control](https://github.com/q and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2). Tested environment: + * Ubuntu 24.04 - * ROS2 Jazzy + * ROS2 Jazzy [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/) @@ -14,20 +15,20 @@ Tested environment: Required hardware interfaces: * command: - * joint position - * joint velocity - * joint effort - * KP - * KD + * joint position + * joint velocity + * joint effort + * KP + * KD * state: - * joint effort - * joint position - * joint velocity - * imu sensor - * linear acceleration - * angular velocity - * orientation - * feet force sensor + * joint effort + * joint position + * joint velocity + * imu sensor + * linear acceleration + * angular velocity + * orientation + * feet force sensor ## 2. Build @@ -47,23 +48,21 @@ colcon build --packages-up-to ocs2_quadruped_controller ``` ## 3. Launch -* Unitree Go1 Robot - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch go1_description ocs2_control.launch.py - ``` -* Unitree Aliengo Robot - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch aliengo_description ocs2_control.launch.py - ``` -* Unitree Go2 Robot - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch go2_description ocs2_control.launch.py - ``` -* Unitree B2 Robot - ```bash - source ~/ros2_ws/install/setup.bash - ros2 launch b2_description ocs2_control.launch.py - ``` \ No newline at end of file + +supported robot description: + +* Unitree + * go2_description + * go1_description + * a1_description + * aliengo_description + * b2_description +* Xiaomi + * cyberdog_description + +### 3.1 Mujoco Simulation + +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=go2_description +``` \ No newline at end of file diff --git a/descriptions/anybotics/anymal_c_description/launch/ocs2_control.launch.py b/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py similarity index 80% rename from descriptions/anybotics/anymal_c_description/launch/ocs2_control.launch.py rename to controllers/ocs2_quadruped_controller/launch/mujoco.launch.py index 41dbcf2..fb8f9a5 100644 --- a/descriptions/anybotics/anymal_c_description/launch/ocs2_control.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/mujoco.launch.py @@ -3,26 +3,21 @@ import os import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -package_description = "anymal_c_description" package_controller = "ocs2_quadruped_controller" - -def process_xacro(): +def launch_setup(context, *args, **kwargs): + package_description = context.launch_configurations['pkg_description'] pkg_path = os.path.join(get_package_share_directory(package_description)) + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() + robot_description = xacro.process_file(xacro_file).toxml() -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -31,6 +26,16 @@ def generate_launch_description(): ] ) + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -59,6 +64,9 @@ def generate_launch_description(): 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2', 'gait.info') }], + remappings=[ + ("~/robot_description", "/robot_description"), + ], output="both", ) @@ -82,15 +90,8 @@ def generate_launch_description(): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - + return [ + rviz, robot_state_publisher, controller_manager, joint_state_publisher, @@ -106,4 +107,17 @@ def generate_launch_description(): on_exit=[ocs2_controller], ) ), + ] + + +def generate_launch_description(): + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' + ) + + return LaunchDescription([ + pkg_description, + OpaqueFunction(function=launch_setup), ]) diff --git a/controllers/unitree_guide_controller/README.md b/controllers/unitree_guide_controller/README.md index 86216a7..9e36d00 100644 --- a/controllers/unitree_guide_controller/README.md +++ b/controllers/unitree_guide_controller/README.md @@ -5,6 +5,7 @@ found [here](https://github.com/unitreerobotics/unitree_guide). I used KDL for t the controller performance has difference with the original one (sometimes very unstable). Tested environment: + * Ubuntu 24.04 * ROS2 Jazzy * Ubuntu 22.04 @@ -27,12 +28,34 @@ Required hardware interfaces: * joint position * joint velocity * imu sensor - * linear acceleration - * angular velocity - * orientation + * linear acceleration + * angular velocity + * orientation ## 2. Build + ```bash cd ~/ros2_ws colcon build --packages-up-to unitree_guide_controller +``` + +## 3. Launch + +### 3.1 Mujoco Simulation + +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=go2_description +``` + +### 3.2 Gazebo Classic 11 (ROS2 Humble) +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=go2_description +``` + +### 3.3 Gazebo Harmonic (ROS2 Jazzy) +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=go2_description ``` \ No newline at end of file diff --git a/controllers/unitree_guide_controller/launch/mujoco.launch.py b/controllers/unitree_guide_controller/launch/mujoco.launch.py index 60c084e..7d7486b 100644 --- a/controllers/unitree_guide_controller/launch/mujoco.launch.py +++ b/controllers/unitree_guide_controller/launch/mujoco.launch.py @@ -9,12 +9,8 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -package_description = "go2_description" - - def launch_setup(context, *args, **kwargs): - package_description = context.launch_configurations['pkg_description'] pkg_path = os.path.join(get_package_share_directory(package_description)) @@ -113,4 +109,4 @@ def generate_launch_description(): return LaunchDescription([ pkg_description, OpaqueFunction(function=launch_setup), - ]) \ No newline at end of file + ]) diff --git a/descriptions/anybotics/anymal_c_description/README.md b/descriptions/anybotics/anymal_c_description/README.md index f14c550..fad8e27 100644 --- a/descriptions/anybotics/anymal_c_description/README.md +++ b/descriptions/anybotics/anymal_c_description/README.md @@ -25,7 +25,7 @@ ros2 launch anymal_c_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch anymal_c_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description ``` * Legged Gym Controller ```bash diff --git a/descriptions/anybotics/anymal_c_description/config/ocs2/reference.info b/descriptions/anybotics/anymal_c_description/config/ocs2/reference.info index 05ba79c..31788c0 100644 --- a/descriptions/anybotics/anymal_c_description/config/ocs2/reference.info +++ b/descriptions/anybotics/anymal_c_description/config/ocs2/reference.info @@ -1,7 +1,7 @@ targetDisplacementVelocity 0.5; targetRotationVelocity 1.57; -comHeight 0.565; +comHeight 0.6; defaultJointState { diff --git a/descriptions/anybotics/anymal_c_description/config/ocs2/task.info b/descriptions/anybotics/anymal_c_description/config/ocs2/task.info index cc08de7..bb7f5cb 100644 --- a/descriptions/anybotics/anymal_c_description/config/ocs2/task.info +++ b/descriptions/anybotics/anymal_c_description/config/ocs2/task.info @@ -262,10 +262,10 @@ selfCollision ; Self Collision pairs collisionLinkPairs { - [0] "LF_SHANK, RF_SHANK" - [1] "LH_SHANK, RH_SHANK" - [2] "LF_SHANK, LH_SHANK" - [3] "RF_SHANK, RH_SHANK" + [0] "LF_shank_fixed, RF_shank_fixed" + [1] "LH_shank_fixed, RH_shank_fixed" + [2] "LF_shank_fixed, LH_shank_fixed" + [3] "RF_shank_fixed, RH_shank_fixed" [4] "LF_FOOT, RF_FOOT" [5] "LH_FOOT, RH_FOOT" [6] "LF_FOOT, LH_FOOT" @@ -308,7 +308,7 @@ weight ; State Estimation kalmanFilter { - footRadius 0.03 + footRadius 0.015 imuProcessNoisePosition 0.02 imuProcessNoiseVelocity 0.02 footProcessNoisePosition 0.002 diff --git a/descriptions/anybotics/anymal_c_description/config/robot_control.yaml b/descriptions/anybotics/anymal_c_description/config/robot_control.yaml index 1c07eb4..506be9a 100644 --- a/descriptions/anybotics/anymal_c_description/config/robot_control.yaml +++ b/descriptions/anybotics/anymal_c_description/config/robot_control.yaml @@ -26,7 +26,8 @@ imu_sensor_broadcaster: ocs2_quadruped_controller: ros__parameters: - + update_rate: 500 # Hz + default_kd: 1.5 joints: - LF_HAA - LF_HFE diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index eba37f7..348b369 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -30,7 +30,7 @@ ros2 launch a1_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch a1_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=a1_description ``` * RL Quadruped Controller ```bash diff --git a/descriptions/unitree/a1_description/launch/ocs2_control.launch.py b/descriptions/unitree/a1_description/launch/ocs2_control.launch.py deleted file mode 100644 index dde8f81..0000000 --- a/descriptions/unitree/a1_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "a1_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ]) diff --git a/descriptions/unitree/aliengo_description/README.md b/descriptions/unitree/aliengo_description/README.md index 8ae34c1..0b0b833 100644 --- a/descriptions/unitree/aliengo_description/README.md +++ b/descriptions/unitree/aliengo_description/README.md @@ -37,9 +37,9 @@ ros2 launch aliengo_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch aliengo_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=aliengo_description ``` - + ### Gazebo Simulator * Unitree Guide Controller diff --git a/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py b/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py deleted file mode 100644 index 88ab2ea..0000000 --- a/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "aliengo_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ]) diff --git a/descriptions/unitree/b2_description/README.md b/descriptions/unitree/b2_description/README.md index db8d3bf..1bea8c8 100644 --- a/descriptions/unitree/b2_description/README.md +++ b/descriptions/unitree/b2_description/README.md @@ -32,9 +32,9 @@ ros2 launch b2_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch b2_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=b2_description ``` - + ### Gazebo Simulator * Unitree Guide Controller diff --git a/descriptions/unitree/b2_description/launch/ocs2_control.launch.py b/descriptions/unitree/b2_description/launch/ocs2_control.launch.py deleted file mode 100644 index e151a14..0000000 --- a/descriptions/unitree/b2_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "b2_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ]) diff --git a/descriptions/unitree/go1_description/README.md b/descriptions/unitree/go1_description/README.md index f26b0dd..8e08652 100644 --- a/descriptions/unitree/go1_description/README.md +++ b/descriptions/unitree/go1_description/README.md @@ -32,7 +32,7 @@ ros2 launch go1_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch go1_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=go1_description ``` ### Gazebo Simulator diff --git a/descriptions/unitree/go1_description/launch/ocs2_control.launch.py b/descriptions/unitree/go1_description/launch/ocs2_control.launch.py deleted file mode 100644 index 8ecf4f1..0000000 --- a/descriptions/unitree/go1_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "go1_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ]) diff --git a/descriptions/unitree/go2_description/launch/gazebo_classic.launch.py b/descriptions/unitree/go2_description/launch/gazebo_classic.launch.py deleted file mode 100644 index 2962525..0000000 --- a/descriptions/unitree/go2_description/launch/gazebo_classic.launch.py +++ /dev/null @@ -1,115 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch.launch_description_sources import PythonLaunchDescriptionSource - -package_description = "go2_description" - - - -def launch_setup(context, *args, **kwargs): - - package_description = context.launch_configurations['pkg_description'] - pkg_path = os.path.join(get_package_share_directory(package_description)) - - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true', 'CLASSIC': 'true'}).toxml() - - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ) - - gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])] - ), - launch_arguments={"verbose": "false"}.items(), - ) - - spawn_entity = Node( - package="gazebo_ros", - executable="spawn_entity.py", - arguments=["-topic", "robot_description", "-entity", "robot", "-z", "0.5"], - output="screen", - ) - - 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 - } - ], - ) - - 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"], - ) - - unitree_guide_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"], - ) - - return [ - rviz, - robot_state_publisher, - gazebo, - spawn_entity, - leg_pd_controller, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], - ) - ), - ] - - -def generate_launch_description(): - pkg_description = DeclareLaunchArgument( - 'pkg_description', - default_value='go2_description', - description='package for robot description' - ) - - return LaunchDescription([ - pkg_description, - OpaqueFunction(function=launch_setup), - ]) \ No newline at end of file diff --git a/descriptions/unitree/go2_description/launch/ocs2_control.launch.py b/descriptions/unitree/go2_description/launch/ocs2_control.launch.py deleted file mode 100644 index dea1799..0000000 --- a/descriptions/unitree/go2_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "go2_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ]) diff --git a/descriptions/xiaomi/cyberdog_description/README.md b/descriptions/xiaomi/cyberdog_description/README.md index 29e9f6b..a62d9be 100644 --- a/descriptions/xiaomi/cyberdog_description/README.md +++ b/descriptions/xiaomi/cyberdog_description/README.md @@ -33,7 +33,7 @@ ros2 launch cyberdog_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch cyberdog_description ocs2_control.launch.py + ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=cyberdog_description ``` ### Gazebo Simulator diff --git a/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info b/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info index 47047e2..9fcd2cd 100644 --- a/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info +++ b/descriptions/xiaomi/cyberdog_description/config/ocs2/reference.info @@ -1,7 +1,7 @@ targetDisplacementVelocity 0.4; targetRotationVelocity 1.57; -comHeight 0.3 +comHeight 0.28; defaultJointState { diff --git a/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info b/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info index d18089c..720d7d6 100644 --- a/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info +++ b/descriptions/xiaomi/cyberdog_description/config/ocs2/task.info @@ -19,7 +19,7 @@ swing_trajectory_config { liftOffVelocity 0.05 touchDownVelocity -0.1 - swingHeight 0.1 + swingHeight 0.08 swingTimeScale 0.15 } @@ -283,9 +283,9 @@ selfCollision ; Whole body control torqueLimitsTask { - (0,0) 33.5 ; HAA - (1,0) 33.5 ; HFE - (2,0) 33.5 ; KFE + (0,0) 12.0 ; HAA + (1,0) 12.0 ; HFE + (2,0) 12.0 ; KFE } frictionConeTask diff --git a/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py b/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py deleted file mode 100644 index 9fed3d6..0000000 --- a/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -import os - -import xacro -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import OpaqueFunction, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -package_description = "cyberdog_description" -package_controller = "ocs2_quadruped_controller" - - -def process_xacro(): - pkg_path = os.path.join(get_package_share_directory(package_description)) - xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') - robot_description_config = xacro.process_file(xacro_file) - return robot_description_config.toxml() - -def generate_launch_description(): - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") - - robot_description = process_xacro() - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - 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 - } - ], - ) - - 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') - }], - output="both", - ) - - 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"], - ) - - ocs2_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] - ) - - return LaunchDescription([ - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ), - - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[ocs2_controller], - ) - ), - ])