Hi all,
I’m currently working on a project involving a Franka Emika FR3 arm, where I’ve successfully integrated cuMotion as the motion planning pipeline. The arm is now planning and executing trajectories correctly on the real robot.
My next goal is to incorporate NVBLOX to allow dynamic obstacle-aware planning. Specifically, I want the cuMotion planner to take into account obstacles perceived in real-time through NVBLOX (e.g., reconstructed 3D TSDF or ESDF maps from depth sensors).
Current Setup:
- Robot: Franka Emika FR3 arm
- Motion Planning: cuMotion (planning and execution verified on real hardware)
- Mapping: NVBLOX (live 3D mapping running from depth camera)
- Middleware: ROS 2 Humble
- Environment: Ubuntu 22.04 + CUDA 12.9
camera- realsense d455
i am launching nvblox realsense_example.launch.py which gives this realsense_example.launch.py
And this is my Movit launch with cumotion
What I Need Help With:
- Connecting NVBLOX to cuMotion:
- How can I feed NVBLOX’s ESDF map into cuMotion so that it considers obstacles during trajectory planning?
- Does cuMotion support an obstacle cost layer or ESDF interface for dynamic planning?
- Typical Integration Pipeline:
- Are there recommended nodes or examples for setting up this integration?
- Should I be converting the ESDF into a collision checker (e.g., signed distance field or voxel grid)?
- cuMotion Planning with Live Map:
- Can cuMotion natively consume NVBLOX outputs or will I need to bridge it through a different representation like OctoMap or custom collision checker?
- General Advice:
- Any tips, best practices, or working examples for integrating obstacle-aware planning using NVBLOX with a manipulator?
I would really appreciate any guidance, sample code, or architectural advice. Thank you!
My movit.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
Shutdown
)
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution
)
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
import yaml
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, 'r') as file:
return yaml.safe_load(file)
except EnvironmentError:
return None
def generate_launch_description():
robot_ip_parameter_name = 'robot_ip'
use_fake_hardware_parameter_name = 'use_fake_hardware'
fake_sensor_commands_parameter_name = 'fake_sensor_commands'
namespace_parameter_name = 'namespace'
use_isaac_sim_parameter_name = 'use_isaac_sim'
robot_ip = LaunchConfiguration(robot_ip_parameter_name)
use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
namespace = LaunchConfiguration(namespace_parameter_name)
use_isaac_sim = LaunchConfiguration(use_isaac_sim_parameter_name)
# Declare launch arguments
launch_args = [
DeclareLaunchArgument('db', default_value='False', description='Database flag'),
DeclareLaunchArgument(robot_ip_parameter_name, description='Hostname or IP address of the robot.'),
DeclareLaunchArgument(namespace_parameter_name, default_value='', description='Namespace for the robot.'),
DeclareLaunchArgument(use_fake_hardware_parameter_name, default_value='false', description='Use fake hardware'),
DeclareLaunchArgument(
fake_sensor_commands_parameter_name,
default_value='false',
description="Fake sensor commands. Only valid when '{}' is true".format(use_fake_hardware_parameter_name)),
DeclareLaunchArgument(use_isaac_sim_parameter_name, default_value='false', description='Use Isaac Sim topic-based interface'),
]
# Robot Description (URDF + SRDF)
franka_xacro_file = os.path.join(
get_package_share_directory('franka_description'),
'robots', 'fr3', 'fr3.urdf.xacro'
)
robot_description_config = Command([
FindExecutable(name='xacro'), ' ',
franka_xacro_file,
' hand:=true',
' robot_ip:=', robot_ip,
' use_fake_hardware:=', use_fake_hardware,
' fake_sensor_commands:=', fake_sensor_commands,
' use_isaac_sim:=', use_isaac_sim,
' ros2_control:=true'
])
robot_description = {'robot_description': ParameterValue(robot_description_config, value_type=str)}
franka_semantic_xacro_file = os.path.join(
get_package_share_directory('franka_description'),
'robots', 'fr3', 'fr3.srdf.xacro'
)
robot_description_semantic_config = Command(
[FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true']
)
robot_description_semantic = {'robot_description_semantic': ParameterValue(
robot_description_semantic_config, value_type=str)}
kinematics_yaml = load_yaml(
'franka_fr3_moveit_config', 'config/kinematics.yaml'
)
# OMPL Planning Pipeline
ompl_planning_pipeline_config = {
'move_group': {
'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters': (
'default_planner_request_adapters/AddTimeOptimalParameterization '
'default_planner_request_adapters/ResolveConstraintFrames '
'default_planner_request_adapters/FixWorkspaceBounds '
'default_planner_request_adapters/FixStartStateBounds '
'default_planner_request_adapters/FixStartStateCollision '
'default_planner_request_adapters/FixStartStatePathConstraints'),
'start_state_max_bounds_error': 0.1,
}
}
ompl_planning_yaml = load_yaml(
'franka_fr3_moveit_config', 'config/ompl_planning.yaml'
)
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
# cuMotion planning pipeline
cumotion_planning_yaml = load_yaml(
'isaac_ros_cumotion_moveit', 'config/isaac_ros_cumotion_planning.yaml'
)
planning_pipeline_config = {
'planning_pipelines': ['isaac_ros_cumotion', 'ompl'],
'default_planning_pipeline': 'isaac_ros_cumotion',
'isaac_ros_cumotion': cumotion_planning_yaml,
'ompl': ompl_planning_pipeline_config['move_group'],
}
moveit_simple_controllers_yaml = load_yaml(
'franka_fr3_moveit_config', 'config/fr3_controllers.yaml'
)
moveit_controllers = {
'moveit_simple_controller_manager': moveit_simple_controllers_yaml,
'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager',
}
trajectory_execution = {
'moveit_manage_controllers': True,
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
'trajectory_execution.allowed_goal_duration_margin': 0.5,
'trajectory_execution.allowed_start_tolerance': 0.01,
}
planning_scene_monitor_parameters = {
'publish_planning_scene': True,
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True,
'monitor_environment': True,
'monitor_dynamic_environment': True,
'octomap_frame': 'odom', # Use correct global frame
'octomap_resolution': 0.05,
'point_cloud_topic': '/octomap_point_cloud_centers',
}
# Nodes
run_move_group_node = Node(
package='moveit_ros_move_group',
executable='move_group',
namespace=namespace,
output='screen',
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
rviz_config_file = os.path.join(
get_package_share_directory('franka_fr3_moveit_config'),
'rviz',
'moveit.rviz'
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
planning_pipeline_config,
kinematics_yaml,
],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='both',
parameters=[robot_description],
)
ros2_controllers_path = os.path.join(
get_package_share_directory('franka_fr3_moveit_config'),
'config',
'fr3_ros_controllers.yaml',
)
ros2_control_node = Node(
package='controller_manager',
executable='ros2_control_node',
namespace=namespace,
parameters=[robot_description, ros2_controllers_path],
remappings=[('joint_states', 'franka/joint_states')],
output={'stdout': 'screen', 'stderr': 'screen'},
on_exit=Shutdown(),
)
# Load Controllers
load_controllers = []
for controller in ['fr3_arm_controller', 'fr3_gripper_controller', 'joint_state_broadcaster']:
load_controllers.append(
ExecuteProcess(
cmd=[
'ros2', 'run', 'controller_manager', 'spawner', controller,
'--controller-manager-timeout', '60',
'--controller-manager',
PathJoinSubstitution([namespace, 'controller_manager'])
],
output='screen'
)
)
joint_state_publisher = Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
namespace=namespace,
parameters=[{
'source_list': ['franka/joint_states', 'fr3_gripper/joint_states'],
'rate': 30
}],
)
return LaunchDescription(launch_args + [
rviz_node,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
joint_state_publisher,
] + load_controllers)
Best regards,