Plan failed due to Start or End state in collision

[Spphire]

python version: 3.9 Isaac Sim version 2022

[WARNING] [curobo] Start or End state in collision
2024-02-19 07:28:39 [45,901ms] [Warning] [__main__] Plan did not converge to a solution.  No action is being taken.

It's weird cause I set world_config empty and cut the robot(shadowhand)'s collision ball number to 2, plus they can't collide.

All the files needed are below: pack.zip

[balakumar-s]

The start state could also be invalid if it's out of joint limits. Can you check if that's the case?

[Spphire]

The start state could also be invalid if it's out of joint limits. Can you check if that's the case?

After checking, I think the retract config are all in the joint limits. Some of them are 0.0 which are at bounds, but after I set it to 0.01, the problem remains.

[balakumar-s]

Try in the latest commit.

[Spphire]

Try in the latest commit.

It didn't show errors about collision but still plan failed, I recorded target tcp from the specific joint value I set, so it could be because there are too many targets(link_poses) that lead to a bad fit

[Spphire]

situation1: plan without link_poses for example:

result = motion_gen.plan_single(
                cu_js.unsqueeze(0), ik_goal, plan_config.clone(), # link_poses=link_poses
            )

and in robot config, link_names: []

plan successfully with all five fingers

[Spphire]

situation2: plan without link_poses for example:

result = motion_gen.plan_single(
                cu_js.unsqueeze(0), ik_goal, plan_config.clone(), # link_poses=link_poses
            )

and in robot config add 1 finger different from ee_link like:

ee_link: "thdistal" # thumb
link_names: ["ffdistal"] # index finger

plan failed

2024-03-01 07:45:57 [14,640ms] [Warning] [__main__] Plan did not converge to a solution.  No action is being taken.

[Spphire]

situation2: plane without link_poses for example:

result = motion_gen.plan_single(
                cu_js.unsqueeze(0), ik_goal, plan_config.clone(), # link_poses=link_poses
            )

and in robot config add 1 finger different from ee_link like:

ee_link: "thdistal" # thumb
link_names: ["ffdistal"] # index finger

plan failed

2024-03-01 07:45:57 [14,640ms] [Warning] [__main__] Plan did not converge to a solution.  No action is being taken.

plan_config:

plan_config = MotionGenPlanConfig(
        enable_graph=False, enable_graph_attempt=8, max_attempts=20, enable_finetune_trajopt=True
    )

[Spphire]

situation3: plan with link_poses palm+1 finger plan failed

link_poses dict_keys(['ffdistal'])
[WARNING] [curobo] Couldn't find solution with 10 attempts, resetting seeds
2024-03-01 07:55:58 [19,165ms] [Warning] [__main__] Plan did not converge to a solution.  No action is being taken.

[Spphire]

If in robot config, ee_link palm(not finger) and without link_names:

ee_link: "palm"
link_names: []

Regardless of whether the "link_names" parameter is added to the "plan single" function, an error will be reported. Is it normal?

Warp CUDA error 700: an illegal memory access was encountered (/buildAgent/work/a9ae500d09a78409/warp/native/warp.cu:1533)
Traceback (most recent call last):
  File "examples/isaac_sim/multi_arm_reacher_shadowhand.py", line 422, in <module>
    main()
  File "examples/isaac_sim/multi_arm_reacher_shadowhand.py", line 144, in main
    motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/motion_gen.py", line 2226, in warmup
    link_poses=link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1323, in plan_single
    link_poses=link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1127, in _plan_attempts
    link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/motion_gen.py", line 1440, in _plan_from_solve_state
    link_poses,
  File "/home/robotflow/workspace/ov/pkg/isaac_sim-2022.2.1/kit/python/lib/python3.7/contextlib.py", line 74, in inner
    return func(*args, **kwds)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/motion_gen.py", line 958, in _solve_ik_from_solve_state
    link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/ik_solver.py", line 404, in solve_any
    link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/ik_solver.py", line 487, in solve_single
    link_poses=link_poses,
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/reacher/ik_solver.py", line 686, in solve_from_solve_state
    result = self.solver.solve(goal_buffer, coord_position_seed)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/wrap_base.py", line 145, in solve
    act_seq = self.optimize(seed, shift_steps=0)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/wrap/wrap_base.py", line 71, in optimize
    act_seq = opt.optimize(act_seq, shift_steps)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/opt/opt_base.py", line 170, in optimize
    out = self._optimize(opt_tensor, shift_steps, n_iters)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/opt/newton/newton_base.py", line 142, in _optimize
    self._initialize_opt_iters_graph(q, grad_q, shift_steps=shift_steps)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/opt/newton/newton_base.py", line 492, in _initialize_opt_iters_graph
    self._create_opt_iters_graph(q, grad_q, shift_steps)
  File "/home/robotflow/workspace/curobo-newest/curobo/src/curobo/opt/newton/newton_base.py", line 539, in _create_opt_iters_graph
    with torch.cuda.graph(self.cu_opt_graph, stream=s):
  File "/home/robotflow/workspace/ov/pkg/isaac_sim-2022.2.1/extscache/omni.pip.torch-1_13_1-0.1.4+104.2.lx64/torch-1-13-1/torch/cuda/graphs.py", line 147, in __enter__
    torch.cuda.synchronize()
  File "/home/robotflow/workspace/ov/pkg/isaac_sim-2022.2.1/extscache/omni.pip.torch-1_13_1-0.1.4+104.2.lx64/torch-1-13-1/torch/cuda/__init__.py", line 566, in synchronize
    return torch._C._cuda_synchronize()
RuntimeError: CUDA error: an illegal memory access was encountered
CUDA kernel errors might be asynchronously reported at some other API call,so the stacktrace below might be incorrect.
For debugging consider passing CUDA_LAUNCH_BLOCKING=1.
2024-03-01 07:59:19 [10,300ms] [Warning] [carb.audio.context] 1 contexts were leaked
/home/robotflow/workspace/ov/pkg/isaac_sim-2022.2.1/python.sh: line 41: 21664 Segmentation fault      (core dumped) $python_exe "$@" $args
There was an error running python

[balakumar-s]

Can you try with link_names:null and also not sending link_poses to plan_single ?