Issues with planning without using Cartesian Path

SagarJoshi73249 commented 2 months ago

I installed cuMotion to use with a kuka iiwa 14 and set it up for it. I have it working now but it fails to plan when 'Use Cartesian Path' is not ticked in Rviz. Is there any work around for this? I'll attach the error that pops up when trying to plan without using cartesian path.

lm-2023@lm2023-XPS-8950:~/test_cumotion$ ros2 run isaac_ros_cumotion cumotion_planner_node --ros-args -p robot:=/home/lm-2023/test_cumotion/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa14/iiwa14.xrdf -p urdf_path:=/home/lm-2023/test_cumotion/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa14/iiwa14.urdf [INFO] [1719444122.942008693] [cumotion_action_server]: Loaded robot file name: /home/lm-2023/test_cumotion/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa14/iiwa14.xrdf [INFO] [1719444122.942214286] [cumotion_action_server]: warming up cuMotion, wait until ready [INFO] [1719444131.745964384] [cumotion_action_server]: cuMotion is ready for planning queries! [INFO] [1719444134.438150994] [cumotion_action_server]: Executing goal... [INFO] [1719444134.438439348] [cumotion_action_server]: Planning with time_dilation_factor: 0.1 [INFO] [1719444134.661215102] [cumotion_action_server]: Calculating goal pose from Joint target [ERROR] [1719444135.750506511] [cumotion_action_server]: Motion planning failed wih status: MotionGenStatus.IK_FAIL [INFO] [1719444135.750720822] [cumotion_action_server]: returned planning result (query, success, failure_status): 0 False MotionGenStatus.IK_FAIL [INFO] [1719444608.355688643] [cumotion_action_server]: Executing goal... [INFO] [1719444608.356007402] [cumotion_action_server]: Planning with time_dilation_factor: 0.7999999999999999 [INFO] [1719444608.479848420] [cumotion_action_server]: Calculating goal pose from Joint target [ERROR] [1719444608.597395230] [cumotion_action_server]: Motion planning failed wih status: MotionGenStatus.IK_FAIL [INFO] [1719444608.597599248] [cumotion_action_server]: returned planning result (query, success, failure_status): 1 False MotionGenStatus.IK_FAIL [INFO] [1719444613.165260666] [cumotion_action_server]: Executing goal... [INFO] [1719444613.165448058] [cumotion_action_server]: Planning with time_dilation_factor: 0.7999999999999999 [INFO] [1719444613.188976521] [cumotion_action_server]: Calculating goal pose from Joint target [ERROR] [1719444613.300986901] [cumotion_action_server]: Motion planning failed wih status: MotionGenStatus.IK_FAIL

Thank you!

shengyangzhuang commented 1 month ago

Dear SagarJoshi73249,

I am also using LBR-stack but for KUKA iiwa7. I am also trying to configure it with cuMotion. However, my configuration is always stuck on planning even though there isn't any error. Have you ever had this problem while having it work with iiwa 14? I also need to use cartesian path later and maybe I can debug with you together. I am sorry for mentioning something not relevant to your solution but I would be appreciated for any insight you can provide.

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

Thanks.

With warm regards,

shengyangzhuang

SagarJoshi73249 commented 1 month ago

Hey shengyangzhuang,

I also faced something similar when not using Cartesian Path. I figured out the solution for my issue, and it was actually related to the xrdf file that I had created using IsaacSim. You need to be very specific with the collision spheres as well as the ignore parameters for self collisions. Once you get it right you shouldn’t face any issues regarding the planning.

Best, Sagar Joshi.

shengyangzhuang commented 1 month ago

Hey shengyangzhuang,

I also faced something similar when not using Cartesian Path. I figured out the solution for my issue, and it was actually related to the xrdf file that I had created using IsaacSim. You need to be very specific with the collision spheres as well as the ignore parameters for self collisions. Once you get it right you shouldn’t face any issues regarding the planning.

Best, Sagar Joshi.

Update:

The XRDF only works if "use cartesian path" is ticked off. It still gets stuck on planning if that is not ticked.

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

NVIDIA-ISAAC-ROS / isaac_ros_cumotion

Issues with planning without using Cartesian Path #6