Moveit2 on Isaac Sim with custom robot( turtlebot lime)

Omniverse Isaac Sim
, , , ,
1

Hi,

I’m trying to implement MoveIt2 on Isaac Sim with a custom robot, but I’m having some issues. I was able to do it in a real environment, but I’m encountering problems in the simulation.

I can control the robot arm through the Isaac Sim Physics Inspector, but I cannot implement Plan and Execute with MoveIt2.

I will upload my environment USD file and code Here.
#contain nav2_example.usd , lime_usd and src (for ros2 colcon build)

Issue: Planning and Execute have failed.

Step:

  1. play on Isaac sim
  2. run ros2 launch turtlebot3_lime_moveit_config moveit_core.launch.py
  3. Plan & Execute

Video:

Error:
[move_group-2] [INFO] [1729667211.028810631] [moveit.ompl_planning.model_based_planning_context]: Planner configuration ‘arm’ will use planner ‘geometric::RRTConnect’. Additional configuration parameters will be set when the planner is constructed.
[move_group-2] [WARN] [1729667211.058517002] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[move_group-2] [INFO] [1729667211.061928987] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[rviz2-1] [INFO] [1729667211.062841500] [move_group_interface]: Planning request complete!
[rviz2-1] [INFO] [1729667211.063811730] [move_group_interface]: time taken to generate plan: 0.0291518 seconds
[move_group-2] [INFO] [1729667213.449659936] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-2] [INFO] [1729667213.449987627] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-2] [INFO] [1729667213.450066769] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-2] [INFO] [1729667213.450105545] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[rviz2-1] [INFO] [1729667213.449993945] [move_group_interface]: Execute request accepted
[move_group-2] [INFO] [1729667213.450277792] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-2] [INFO] [1729667214.450368988] [moveit_ros.current_state_monitor]: Didn’t receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1729667213.450291, but latest received state has time 148.583341.
[move_group-2] Check clock synchronization if your are running ROS across multiple machines!
[move_group-2] [WARN] [1729667214.450485935] [moveit_ros.trajectory_execution_manager]: Failed to validate trajectory: couldn’t receive full current joint state within 1s
[move_group-2] [INFO] [1729667214.450540134] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED
[rviz2-1] [INFO] [1729667214.451078035] [move_group_interface]: Execute request aborted
[rviz2-1] [ERROR] [1729667214.451165542] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached

3

Welcome back @Misoka_An!

Thanks for your log. I noticed this issue from your log:

[move_group-2] [INFO] [1729667214.450368988] [moveit_ros.current_state_monitor]: Didn’t receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1729667213.450291, but latest received state has time 148.583341.

It looks like the timestamp is not correct which prohibits the plan from executing. The request time is 1729667213.450291 which is system time while the latest received state is time 148.583341 which is simulation time. The latest received state probably comes from your simulation so the simulation time makes sense. But the requested time should be simulation time instead of system time as well.

You probably need to look into any param files and see if you set the parameter use_sim_time to True. If you have done that but this issue still persist, you might need to look into the corresponding node which generates the error (I suppose it is current_state_monitor) and trace down why the timestamp is not correct.

4

Hi @zhengwang!

Thanks again for taking a look. After setting use__sim_time=true, the error is gone, but I still cannot execute commands from MoveIt2.

Right now, I’m working on servo and teleop to check if Isaac Sim and ROS are fully connected.

5

I might be off here, but you can check the list of controllers with ros2 control list_controllers, maybe try another controller or maybe you need to launch the control stack separately

6

Hi @edsonbffilho

That could be the case. After launching my robot bringup code, it looks like /controller_manager/list_controllers was not launched properly.

file:

Github repository:
GitHub - ROBOTIS-JAPAN-GIT/turtlebot3_lime: TurtleBot3 Lime packages

log 1: ros2 launch turtlebot3_lime_bringup sim.launch.py:

ros2 launch turtlebot3_lime_bringup sim.launch.py use_fake_hardware:=true use_sim:=true fake_sensor_commands:=true
[INFO] [launch]: All log files can be found below /home/an/.ros/log/2024-11-06-12-05-28-896048-anpc-2996457
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [2996471]
[INFO] [spawner-2]: process started with pid [2996473]
[robot_state_publisher-1] [INFO] [1730862329.103262515] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-1] [INFO] [1730862329.103333256] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1730862329.103340134] [robot_state_publisher]: got segment base_scan
[robot_state_publisher-1] [INFO] [1730862329.103344724] [robot_state_publisher]: got segment battery_link
[robot_state_publisher-1] [INFO] [1730862329.103350172] [robot_state_publisher]: got segment camera_bottom_screw_frame
[robot_state_publisher-1] [INFO] [1730862329.103355743] [robot_state_publisher]: got segment camera_color_frame
[robot_state_publisher-1] [INFO] [1730862329.103360588] [robot_state_publisher]: got segment camera_color_optical_frame
[robot_state_publisher-1] [INFO] [1730862329.103365651] [robot_state_publisher]: got segment camera_depth_frame
[robot_state_publisher-1] [INFO] [1730862329.103370609] [robot_state_publisher]: got segment camera_depth_optical_frame
[robot_state_publisher-1] [INFO] [1730862329.103375328] [robot_state_publisher]: got segment camera_infra1_frame
[robot_state_publisher-1] [INFO] [1730862329.103380050] [robot_state_publisher]: got segment camera_infra1_optical_frame
[robot_state_publisher-1] [INFO] [1730862329.103384683] [robot_state_publisher]: got segment camera_infra2_frame
[robot_state_publisher-1] [INFO] [1730862329.103389651] [robot_state_publisher]: got segment camera_infra2_optical_frame
[robot_state_publisher-1] [INFO] [1730862329.103394212] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-1] [INFO] [1730862329.103398748] [robot_state_publisher]: got segment caster_back_left_link
[robot_state_publisher-1] [INFO] [1730862329.103403750] [robot_state_publisher]: got segment caster_back_right_link
[robot_state_publisher-1] [INFO] [1730862329.103408210] [robot_state_publisher]: got segment dummy_mimic_fix
[robot_state_publisher-1] [INFO] [1730862329.103413046] [robot_state_publisher]: got segment end_effector_link
[robot_state_publisher-1] [INFO] [1730862329.103418159] [robot_state_publisher]: got segment gripper_left_link
[robot_state_publisher-1] [INFO] [1730862329.103422869] [robot_state_publisher]: got segment gripper_right_link
[robot_state_publisher-1] [INFO] [1730862329.103427681] [robot_state_publisher]: got segment imu_link
[robot_state_publisher-1] [INFO] [1730862329.103432777] [robot_state_publisher]: got segment link1
[robot_state_publisher-1] [INFO] [1730862329.103437835] [robot_state_publisher]: got segment link2
[robot_state_publisher-1] [INFO] [1730862329.103443027] [robot_state_publisher]: got segment link3
[robot_state_publisher-1] [INFO] [1730862329.103447746] [robot_state_publisher]: got segment link4
[robot_state_publisher-1] [INFO] [1730862329.103452839] [robot_state_publisher]: got segment link5
[robot_state_publisher-1] [INFO] [1730862329.103457569] [robot_state_publisher]: got segment link6
[robot_state_publisher-1] [INFO] [1730862329.103462345] [robot_state_publisher]: got segment link7
[robot_state_publisher-1] [INFO] [1730862329.103467013] [robot_state_publisher]: got segment wheel_left_link
[robot_state_publisher-1] [INFO] [1730862329.103471660] [robot_state_publisher]: got segment wheel_right_link
[spawner-2] [INFO] [1730862329.342363043] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[spawner-2] [WARN] [1730862339.355716983] [spawner_joint_state_broadcaster]: Could not contact service /controller_manager/list_controllers
[spawner-2] [INFO] [1730862339.356131502] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[spawner-2] [WARN] [1730862349.368493470] [spawner_joint_state_broadcaster]: Could not contact service /controller_manager/list_controllers

log 2: ros2 run controller_manager spawner controller_names:

an@anpc:~/Documents/turtlebot/turtlebot3_ws$ ros2 run controller_manager spawner controller_names
[INFO] [1730865150.526198913] [spawner_controller_names]: waiting for service /controller_manager/list_controllers to become available...
[WARN] [1730865161.528172820] [spawner_controller_names]: Failed getting a result from calling /controller_manager/list_controllers in 0. (Attempt 1 of 3.)
[WARN] [1730865171.528877421] [spawner_controller_names]: Failed getting a result from calling /controller_manager/list_controllers in 0. (Attempt 2 of 3.)
[WARN] [1730865181.529595892] [spawner_controller_names]: Failed getting a result from calling /controller_manager/list_controllers in 0. (Attempt 3 of 3.)
Traceback (most recent call last):
  File "/opt/ros/humble/lib/controller_manager/spawner", line 33, in <module>
    sys.exit(load_entry_point('controller-manager==2.43.1', 'console_scripts', 'spawner')())
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 200, in main
    if is_controller_loaded(
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/spawner.py", line 80, in is_controller_loaded
    controllers = list_controllers(node, controller_manager, service_timeout).controller
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/controller_manager_services.py", line 109, in list_controllers
    return service_caller(
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/controller_manager/controller_manager_services.py", line 90, in service_caller
    raise RuntimeError(
RuntimeError: Could not successfully call service /controller_manager/list_controllers after 3 attempts.
[ros2run]: Process exited with failure 1
7

For the TurtleBot3 Lime packages, which branch are you on? I don’t find sim.launch.py…

Based on the log, it seems that the node which is supposed to run the service /controller_manager/list_controllers takes a long time to start it.

I found a similar issue here. Maybe that would be helpful?

8

The sim.launch.py file is actually a custom file I created specifically for Isaac Sim, and I haven’t pushed it to the Git repository yet. You can find it in the zip file shared above.

sim.launch.py code 
#!/usr/bin/env python3
#
# Copyright 2022 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Author: Darby Lim, Hye-jong KIM

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import RegisterEventHandler
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command
from launch.substitutions import FindExecutable
from launch.substitutions import LaunchConfiguration
from launch.substitutions import PathJoinSubstitution

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            'start_rviz',
            default_value='false',
            description='Whether execute rviz2'
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            'prefix',
            default_value='""',
            description='Prefix of the joint and link names'
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            'use_sim',
            default_value='false',
            description='Start robot in Gazebo simulation.'
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            'use_fake_hardware',
            default_value='false',
            description='Start robot with fake hardware mirroring command to its states.'
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            'fake_sensor_commands',
            default_value='false',
            description='Enable fake command interfaces for sensors used for simple simulations. \
            Used only if "use_fake_hardware" parameter is true.'
        )
    )

    start_rviz = LaunchConfiguration('start_rviz')
    prefix = LaunchConfiguration('prefix')
    use_sim = LaunchConfiguration('use_sim')
    use_fake_hardware = LaunchConfiguration('use_fake_hardware')
    fake_sensor_commands = LaunchConfiguration('fake_sensor_commands')

    urdf_file = Command(
        [
            PathJoinSubstitution([FindExecutable(name='xacro')]),
            ' ',
            PathJoinSubstitution(
                [
                    FindPackageShare('turtlebot3_lime_description'),
                    'urdf',
                    'turtlebot3_lime.urdf.xacro'
                ]
            ),
            ' ',
            'prefix:=',
            prefix,
            ' ',
            'use_sim:=',
            use_sim,
            ' ',
            'use_fake_hardware:=',
            use_fake_hardware,
            ' ',
            'fake_sensor_commands:=',
            fake_sensor_commands,
        ]
    )

    controller_manager_config = PathJoinSubstitution(
        [
            FindPackageShare('turtlebot3_lime_bringup'),
            'config',
            'arm_controller_manager.yaml',
        ]
    )

    rviz_config_file = PathJoinSubstitution(
        [
            FindPackageShare('turtlebot3_lime_bringup'),
            'rviz',
            'turtlebot3_lime.rviz'
        ]
    )

    control_node = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[
            {'robot_description': urdf_file},
            controller_manager_config
        ],
        remappings=[
            ('~/cmd_vel_unstamped', 'cmd_vel'),
            ('~/odom', 'odom')
        ],
        output="both",
        condition=UnlessCondition(use_sim))

    robot_state_pub_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': urdf_file, 'use_sim_time': use_sim}],
        output='screen'
    )

    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
        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_arm_controller_spawner_after_joint_state_broadcaster_spawner = \
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=joint_state_broadcaster_spawner,
                on_exit=[arm_controller_spawner],
            )
        )

    delay_gripper_controller_spawner_after_joint_state_broadcaster_spawner = \
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=joint_state_broadcaster_spawner,
                on_exit=[gripper_controller_spawner],
            )
        )

    nodes = [
        control_node,
        robot_state_pub_node,
        joint_state_broadcaster_spawner,
        delay_arm_controller_spawner_after_joint_state_broadcaster_spawner,
        delay_gripper_controller_spawner_after_joint_state_broadcaster_spawner,
    ]

    return LaunchDescription(declared_arguments + nodes)
arm_controller_manager.yaml 

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    gripper_controller:
      type: position_controllers/GripperActionController

arm_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    interface_name: position

    command_interfaces:
      - position

    state_interfaces:
      - position
      - velocity

    state_publish_rate: 200.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: true
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

gripper_controller:
  ros__parameters:
    joint: gripper_left_joint

And thank you for the link to the similar issue! I’ll check it out

9

I was able to fix it. The problem was caused by the <ros2_control> and <gazebo> functions in the robot URDF file.

After removing these functions and modifying ros2_control.urdf.xacro and controller_manager.yaml, I was able to run MoveIt on Isaac.

1 Like