Plan Collision-Free Industrial Robot Paths with MoveIt 2 and NVIDIA cuMotion

                      
                     
from typing import List, Dict
import os
import rclpy
from rclpy.node import Node
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from geometry_msgs.msg import Pose

# Configuration
robot_namespace = os.getenv('ROBOT_NAMESPACE', '/robot')

# Initialize the MoveIt Commander
rclpy.init()
robot = RobotCommander(robot_namespace)
scene = PlanningSceneInterface()  
group = MoveGroupCommander('manipulator')

# Function to plan collision-free path
def plan_path(target_pose: Pose) -> Dict[str, str]:
    try:
        group.set_pose_target(target_pose)
        plan = group.plan()
        if plan:
            return {'success': True, 'plan': plan}
        else:
            return {'success': False, 'message': 'Planning failed.'}
    except Exception as e:
        return {'success': False, 'error': str(e)}

# Main execution
if __name__ == '__main__':
    target_pose = Pose()
    target_pose.position.x = 0.5
    target_pose.position.y = 0.0
    target_pose.position.z = 0.5
    target_pose.orientation.w = 1.0

    result = plan_path(target_pose)
    print(result)
    rclpy.shutdown()