ocs2 launch file refact

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -81,7 +81,7 @@ install(
 )
 
 install(
-        DIRECTORY config
+        DIRECTORY config launch
         DESTINATION share/${PROJECT_NAME}/
 )
 

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 position
-  * joint velocity
+    * joint velocity
-  * joint effort
+    * joint effort
-  * KP
+    * KP
-  * KD
+    * KD
 * state:
-  * joint effort
+    * joint effort
-  * joint position
+    * joint position
-  * joint velocity
+    * joint velocity
-  * imu sensor
+    * imu sensor
-    * linear acceleration
+        * linear acceleration
-    * angular velocity
+        * angular velocity
-    * orientation
+        * orientation
-  * feet force sensor
+    * feet force sensor
 
 ## 2. Build
 
@@ -47,23 +48,21 @@ colcon build --packages-up-to ocs2_quadruped_controller
 ```
 
 ## 3. Launch
-* Unitree Go1 Robot
+
-  ```bash
+supported robot description:
-  source ~/ros2_ws/install/setup.bash
+
-  ros2 launch go1_description ocs2_control.launch.py
+* Unitree
-  ```
+    * go2_description
-* Unitree Aliengo Robot
+    * go1_description
-  ```bash
+    * a1_description
-  source ~/ros2_ws/install/setup.bash
+    * aliengo_description
-  ros2 launch aliengo_description ocs2_control.launch.py
+    * b2_description
-  ```
+* Xiaomi
-* Unitree Go2 Robot
+    * cyberdog_description
-  ```bash
+
-  source ~/ros2_ws/install/setup.bash
+### 3.1 Mujoco Simulation
-  ros2 launch go2_description ocs2_control.launch.py
+
-  ```
+```bash
-* Unitree B2 Robot
+source ~/ros2_ws/install/setup.bash
-  ```bash
+ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=go2_description
-  source ~/ros2_ws/install/setup.bash
+```
-  ros2 launch b2_description ocs2_control.launch.py
-  ```

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 launch_setup(context, *args, **kwargs):
-def process_xacro():
+    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)
+    robot_description = xacro.process_file(xacro_file).toxml()
-    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),
@@ -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([
+    return [
-        Node(
+        rviz,
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-
         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),
     ])

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
+        * linear acceleration
-      * angular velocity
+        * angular velocity
-      * orientation
+        * 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
 ```

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),
-    ])
+    ])

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

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
 {

descriptions/anybotics/anymal_c_description/config/ocs2/task.info

@@ -262,10 +262,10 @@ selfCollision
   ; Self Collision pairs
   collisionLinkPairs
   {
-    [0] "LF_SHANK, RF_SHANK"
+    [0] "LF_shank_fixed, RF_shank_fixed"
-    [1] "LH_SHANK, RH_SHANK"
+    [1] "LH_shank_fixed, RH_shank_fixed"
-    [2] "LF_SHANK, LH_SHANK"
+    [2] "LF_shank_fixed, LH_shank_fixed"
-    [3] "RF_SHANK, RH_SHANK"
+    [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

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

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

descriptions/unitree/a1_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])

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

descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])

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

descriptions/unitree/b2_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])

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

descriptions/unitree/go1_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])

descriptions/unitree/go2_description/launch/gazebo_classic.launch.py

@@ -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),
-    ])

descriptions/unitree/go2_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])

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

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
 {

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
+   (0,0)  12.0     ; HAA
-   (1,0)  33.5     ; HFE
+   (1,0)  12.0     ; HFE
-   (2,0)  33.5     ; KFE
+   (2,0)  12.0     ; KFE
 }
 
 frictionConeTask

descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py

@@ -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],
-            )
-        ),
-    ])