Fix gz_ros2_control plugin by adding robot_param_node

src/lerobot_controller/config/so101_controllers.yaml

@@ -1,12 +1,12 @@
 controller_manager:
   ros__parameters:
-    update_rate: 10 # Hz
+    update_rate: 50 # Hz
 
     arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
-    # gripper_controller:
-    #   type: forward_command_controller/ForwardCommandController
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
 
     gripper_controller:
       type: joint_trajectory_controller/JointTrajectoryController
@@ -32,6 +32,15 @@ arm_controller:
     open_loop_control: true
     allow_integration_in_goal_trajectories: true
 
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - "1"
+      - "2"
+      - "3"
+      - "4"
+      - "5"
+
 gripper_controller:
   ros__parameters:
     joints:

src/lerobot_description/launch/so101_gazebo.launch.py

@@ -3,7 +3,8 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, TimerAction, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
 from launch.substitutions import Command, LaunchConfiguration
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
@@ -66,11 +67,58 @@ def generate_launch_description():
         ]
     )
 
+    # Controller Spawners
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+        output="screen",
+    )
+
+    arm_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["arm_controller"],
+        output="screen",
+    )
+
+    gripper_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gripper_controller"],
+        output="screen",
+    )
+
+    # Delay controller spawning to allow Gazebo to fully initialize
+    delayed_joint_state_broadcaster_spawner = TimerAction(
+        period=5.0,
+        actions=[joint_state_broadcaster_spawner]
+    )
+
+    # Spawn arm controller after joint_state_broadcaster
+    delayed_arm_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[arm_controller_spawner],
+        )
+    )
+
+    # Spawn gripper controller after arm controller
+    delayed_gripper_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=arm_controller_spawner,
+            on_exit=[gripper_controller_spawner],
+        )
+    )
+
     return LaunchDescription([
         model_arg,
         gazebo_resource_path,
         robot_state_publisher_node,
         gazebo,
         gz_spawn_entity,
-        gz_ros2_bridge
-    ])
+        gz_ros2_bridge,
+        delayed_joint_state_broadcaster_spawner,
+        delayed_arm_controller_spawner,
+        delayed_gripper_controller_spawner,
+    ])

src/lerobot_description/rviz/display.rviz

@@ -106,7 +106,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: Base
+    Fixed Frame: base
     Frame Rate: 30
   Name: root
   Tools:

src/lerobot_description/urdf/so101_gazebo.xacro

@@ -4,8 +4,9 @@
     <!-- ROS 2 Control -->
     <gazebo>
         <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+            <robot_param_node>robot_state_publisher</robot_param_node>
             <parameters>$(find lerobot_controller)/config/so101_controllers.yaml</parameters>
         </plugin>
     </gazebo>
 
-</robot>
+</robot>

src/lerobot_moveit/config/moveit.rviz

@@ -106,7 +106,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: Base
+    Fixed Frame: base
     Frame Rate: 30
   Name: root
   Tools:

src/lerobot_moveit/launch/so101_full.launch.py

@@ -0,0 +1,184 @@
+"""
+Launch SO101 robot in Gazebo and RViz with MoveIt, all controllers and planners.
+"""
+import os
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+    TimerAction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+    lerobot_description_dir = get_package_share_directory("lerobot_description")
+    lerobot_moveit_dir = get_package_share_directory("lerobot_moveit")
+    so101_urdf_path = os.path.join(lerobot_description_dir, "urdf", "so101.urdf.xacro")
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="true",
+        description="Use simulation time",
+    )
+    use_sim_time = LaunchConfiguration("use_sim_time", default="true")
+
+    gazebo_resource_path = SetEnvironmentVariable(
+        name="GZ_SIM_RESOURCE_PATH",
+        value=str(Path(lerobot_description_dir).parent),
+    )
+
+    robot_description = ParameterValue(
+        Command(["xacro ", so101_urdf_path]),
+        value_type=str,
+    )
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[
+            {"robot_description": robot_description},
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    gazebo_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("ros_gz_sim"),
+                "launch",
+                "gz_sim.launch.py",
+            )
+        ),
+        launch_arguments=[("gz_args", " -v 4 -r empty.sdf ")],
+    )
+
+    spawn_entity = Node(
+        package="ros_gz_sim",
+        executable="create",
+        output="screen",
+        arguments=["-topic", "robot_description", "-name", "so101"],
+    )
+
+    gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
+    )
+
+    # gz_ros2_control loads hardware but NOT controllers. Spawn them after model is in Gazebo.
+    joint_state_spawner = TimerAction(
+        period=5.0,
+        actions=[
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+                parameters=[{"use_sim_time": use_sim_time}],
+                output="screen",
+            ),
+        ],
+    )
+    arm_controller_spawner = TimerAction(
+        period=7.0,
+        actions=[
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=["arm_controller", "--controller-manager", "/controller_manager"],
+                parameters=[{"use_sim_time": use_sim_time}],
+                output="screen",
+            ),
+        ],
+    )
+    gripper_controller_spawner = TimerAction(
+        period=9.0,
+        actions=[
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=["gripper_controller", "--controller-manager", "/controller_manager"],
+                parameters=[{"use_sim_time": use_sim_time}],
+                output="screen",
+            ),
+        ],
+    )
+    # Load and configure forward_position_controller without activating (arm_controller holds interfaces).
+    # Teleop/servo node activates it via switch_controller when needed.
+    forward_position_loader = TimerAction(
+        period=11.0,
+        actions=[
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=["forward_position_controller", "--controller-manager", "/controller_manager", "--inactive"],
+                parameters=[{"use_sim_time": use_sim_time}],
+                output="screen",
+            ),
+        ],
+    )
+
+    moveit_config = (
+        MoveItConfigsBuilder("so101", package_name="lerobot_moveit")
+        .robot_description(file_path=so101_urdf_path)
+        .robot_description_semantic(file_path="config/so101.srdf")
+        .robot_description_kinematics(file_path="config/kinematics.yaml")
+        .joint_limits(file_path="config/joint_limits.yaml")
+        .trajectory_execution(file_path="config/moveit_controllers.yaml")
+        .planning_pipelines(pipelines=["ompl"])
+        .pilz_cartesian_limits(file_path="config/pilz_cartesian_limits.yaml")
+        .to_moveit_configs()
+    )
+
+    move_group = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+            {"use_sim_time": use_sim_time},
+            {"publish_robot_description_semantic": True},
+        ],
+        arguments=["--ros-args", "--log-level", "info"],
+    )
+
+    rviz_config_path = os.path.join(lerobot_moveit_dir, "config", "moveit.rviz")
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        arguments=["-d", rviz_config_path],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+            moveit_config.planning_pipelines,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    return LaunchDescription([
+        use_sim_time_arg,
+        gazebo_resource_path,
+        robot_state_publisher,
+        gazebo_launch,
+        spawn_entity,
+        gz_bridge,
+        joint_state_spawner,
+        arm_controller_spawner,
+        gripper_controller_spawner,
+        forward_position_loader,
+        move_group,
+        rviz_node,
+    ])