Possible Error When Motion_Gen's Plan Fails to Converge to a Solution

[zoctipus]

I have encountered a very strange bugs while I am building scene on top of Curobo.
At some unknown conditions, the motiongen.plan_batch_env() function reports
"Plan did not converge to a solution" for any poses generated.

I have checked:

1. target poses are in local environment frame
2. obstacles worldconfig in all batch envs are built in robot base reference (panda_link0 for franka)

And I have also encountered other strange things:

Initially, for the batched environment, it just not find plans for any poses as before any batch size,(num of envs)

Then, at some point when I run the script again, motiongen.plan_batch_env() starts to find the plan, and the plan executes beautifully and reaches all target poses for all 4 environments. However, this is only true when batch(envs) is less than 5. when envs is greater than 5, the planner starts to not finding any plan for any envs again.

Finally, assuming the bug maybe solved, I started cleaning up non-crucial code, but then I got back to Initial state where it just not find plans for any poses as before for any batch size, even including batch size(2-4).

I debugged using write_motion_gen_log, and it looks like below:

<sample generated by write_motion_gen_log, sampled: env0_seed_1.usd and env1_seed_4.usd>
I have used the dump debug function and observed the sequence it generated as above.

The target pose looks reasonably reachable, the table collision looks at the right place, but no motion is generated. Franka's entire motion sequence hardly moved, and stayed in the almost same place as two pictures above.

My question:
1.Have you had any similar issue before? and what might possibly go wrong?
2.What other debugging suggestion do you have to dig into the problem related to planning related to motiongen?

Thank you.

[zoctipus]

Update on this discussion,

In the scenario of envs = 2-4 using "plan_batch_env", It seems like the issue of fail to converge comes from a typo in the frank_yml "base_link" that I accidently miswrote to "panda_link". After changing the name back to base_link. the converge seems to restore.

In the other scenario of envs >= 5(say envs = 5), with the same setting of each franka in env, it seems like the issue of failure to converge arises from trajopt.py, where, the function _get_result(), the line,
feasible = torch.all(result.metrics.feasible, dim=-1)
is never true, and thus
success = torch.logical_and(feasible, converge)
could not be true for any trajectory

and thus checking traj_result.success before executing plan will skip the plan because all results are false.

If I disable the success checking before the action execution, this is what envs = 5 looks like:

From the gif, it looks like as if the goal state is higher up in the sky but if I check the debug output

The goal positons for all 5 of them looks in the right range, and not in the sky.

Compared to envs = 4, which looks like what should do.

I will keep debug on my own, but some debugging tips or insight will be greatly appreciated!

[zoctipus]

I think the similar problem can be reproduced shipped filed,

"batch_motion_gen_reacher.py"

by changing few lines:

n_envs = 2 ->
n_envs = 5

world_file = ["collision_test.yml", "collision_thin_walls.yml"] ->
world_file = ["collision_test.yml" for i in range(n_envs)]

cmd_plan = [None, None] ->
cmd_plan = [None for i in range(n_envs)]

then running the file will cause no planned trajectory being success, if we commented out the check if result.success[s]:
we can observe

In contrast, if we just change the value of n_envs from 5 to 4
n_envs = 4
all the issue are gone and can be seens from

[balakumar-s]

Thanks for trying this out. I responded in the issue you created as well (#82). Try out the latest commit in main and let us know here if the issue still persists.