Fail to configure cumotion

[shengyangzhuang]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): docker Isaac ros
2. python version: 3.8
3. Isaac Sim version (if using):

---

Issue Details

Dear team,

I configure cuMotion with KUKA iiwa 7 following this instruction. I first launch the moveit for my robot then executed the cuMotion planner node. When I started planning, the simulation got stuck in "planning" if I use cuMotion planner. I would be appreciated for any help you can provide. cuMotion node:

admin@Y9000X:/workspaces/isaac_ros-dev/ros2_ws/lbr-stack$ ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args -p robot:=${ISAAC_ROS_WS}/ros2_ws/lbr-stack/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa7/iiwa7.xrdf -p urdf_path:=${ISAAC_ROS_WS}/ros2_ws/lbr-stack/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa7/iiwa7.urdf
[INFO] [1720706985.440885562] [cumotion_action_server]: Loaded robot file name: /workspaces/isaac_ros-dev/ros2_ws/lbr-stack/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa7/iiwa7.xrdf
[INFO] [1720706985.441180027] [cumotion_action_server]: warming up cuMotion, wait until ready
[INFO] [1720706989.057967701] [cumotion_action_server]: cuMotion is ready for planning queries!

moveit launch file terminal:

[gzserver-1] [INFO] [1720706946.642989970] [lbr.joint_trajectory_controller]: Received new action goal
[gzserver-1] [INFO] [1720706946.643069491] [lbr.joint_trajectory_controller]: Accepted new action goal
[move_group-8] [INFO] [1720706946.643198183] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution
[move_group-8] [INFO] [1720706946.643209989] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[gzserver-1] [INFO] [1720706951.746858483] [lbr.joint_trajectory_controller]: Goal reached, success!
[move_group-8] [INFO] [1720706951.793857085] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller' successfully finished
[move_group-8] [INFO] [1720706951.794030466] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...
[move_group-8] [INFO] [1720706951.802500281] [moveit_move_group_default_capabilities.move_action_capability]: Solution was found and executed.
[rviz2-9] [INFO] [1720706951.802815895] [move_group_interface]: Plan and Execute request complete!
[rviz2-9] [INFO] [1720707021.780017704] [move_group_interface]: MoveGroup action client/server ready
[move_group-8] [INFO] [1720707021.780192345] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-8] [INFO] [1720707021.780266882] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-8] [INFO] [1720707021.780345785] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-8] [INFO] [1720707021.780353014] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'isaac_ros_cumotion'
[rviz2-9] [INFO] [1720707021.780285534] [move_group_interface]: Planning request accepted
[move_group-8] [INFO] [1720707021.780435261] [lbr.move_group]: Planning trajectory

The XRDF only works if "use cartesian path" is ticked off. It still gets stuck on planning if that is not ticked. It doesn't return any error and I find it hard to debug.

I attached my XRDF here:

format: xrdf
format_version: 1.0

default_joint_positions:
  A1: 0.0
  A2: 0.0
  A3: 0.0
  A4: 0.0
  A5: 0.0
  A6: 0.0
  A7: 0.0

cspace:
  joint_names:
    - "A1"
    - "A2"
    - "A3"
    - "A4"
    - "A5"
    - "A6"
    - "A7"

  acceleration_limits: [15.0, 15.0, 15.0, 15.0, 15.0, 15.0, 15.0]
  jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0, 500.0]

tool_frames: ["link_ee"]

collision:
  geometry: "iiwa7_collision_spheres"
  buffer_distance:
    link_1: 0.005
    link_2: 0.005
    link_3: 0.005
    link_4: 0.005
    link_5: 0.005
    link_6: 0.005

self_collision:
  geometry: "iiwa7_collision_spheres"
  buffer_distance:
    link_1: -0.02
    link_2: 0.0
    link_3: 0.0
    link_4: 0.0
    link_5: 0.0
    link_6: 0.0
  ignore:
    link_1: ["link_2", "link_3"]
    link_2: ["link_3", "link_4"]
    link_3: ["link_4", "link_5"]
    link_4: ["link_5", "link_6"]
    link_5: ["link_6"]

geometry:
  iiwa7_collision_spheres:
    spheres:
      link_0:
        - center: [0.0, 0.0, 0.05]
          radius: 0.10
      link_1:
        - center: [0.0, 0.0, 0.0]
          radius: 0.08
        - center: [0.0, -0.05, 0.1]
          radius: 0.07
        - center: [0.0, -0.05, 0.18]
          radius: 0.08
      link_2:
        - center: [0.0, 0.0, 0.0]
          radius: 0.08
        - center: [0.0, 0.02, 0.06]
          radius: 0.07
        - center: [0.0, 0.1, 0.03]
          radius: 0.07
        - center: [0.0, 0.18, 0.0]
          radius: 0.08
      link_3:
        - center: [0.0, 0.0, 0.08]
          radius: 0.08
        - center: [0.0, 0.06, 0.16]
          radius: 0.07
        - center: [0.0, 0.05, 0.22]
          radius: 0.07
      link_4:
        - center: [0.0, 0.0, 0.0]
          radius: 0.08
        - center: [0.0, 0.0, 0.05]
          radius: 0.07
        - center: [0.0, 0.07, 0.05]
          radius: 0.06
        - center: [0.0, 0.11, 0.03]
          radius: 0.06
        - center: [0.0, 0.15, 0.01]
          radius: 0.07
      link_5:
        - center: [0.0, 0.0, 0.02]
          radius: 0.08
        - center: [0.0, 0.03, 0.07]
          radius: 0.06
        - center: [0.0, 0.08, 0.13]
          radius: 0.05
      link_6:
        - center: [0.0, 0.0, 0.0]
          radius: 0.06
        - center: [0.0, 0.0, 0.05]
          radius: 0.08
        - center: [0.0, -0.04, 0.075]
          radius: 0.06
        - center: [0.0, 0.08, 0.06]
          radius: 0.065
        - center: [0.0, 0.16, 0.06]
          radius: 0.05

The robot ros2 stack I used if LBR-stack for KUKA.

https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_cumotion/issues/8