search

NVlabs / curobo

CUDA Accelerated Robot Library
https://curobo.org
Other
798 stars 125 forks source link

Fail to configure cumotion #364

Open shengyangzhuang opened 3 months ago

shengyangzhuang commented 3 months ago

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

image image

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