"Opt Fail" in a kinematically constrained environment though IK is successful

[ravijo]

Thanks for this awesome library. It is awesome.

I am simulating a kinematically constrained environment in which the MotionGen is returning "Opt Fail" though IKSolver is successful. The environment and goal pose are shown below:

Below is the code snippet:

tensor_args = TensorDeviceType()
motion_gen_config = MotionGenConfig.load_from_robot_config(
    robot_cfg,
    world_cfg,
    tensor_args,
    collision_checker_type=CollisionCheckerType.MESH,
    num_trajopt_seeds=12,
    num_graph_seeds=12,
    interpolation_dt=0.05,
    optimize_dt=None,
    trajopt_dt=None,
    trajopt_tsteps=32,
    trim_steps=None,
)

motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=True)

plan_config = MotionGenPlanConfig(
    enable_graph=False,
    enable_graph_attempt=2,
    max_attempts=4,
    enable_finetune_trajopt=True,
    parallel_finetune=True,
)

start_state = JointState.from_position(
    tensor_args.to_device([Q_OUTSIDE_BOX]),
    joint_names=JOINT_NAMES,
)

goal_state = motion_gen.rollout_fn.compute_kinematics(
    JointState.from_position(
        tensor_args.to_device([Q_INSIDE_BOX]),
        joint_names=JOINT_NAMES,
    )
)
goal_pose = Pose(goal_state.ee_pos_seq.squeeze(), quaternion=goal_state.ee_quat_seq.squeeze())

result = motion_gen.plan_single(start_state, goal_pose, plan_config)
ik_result = motion_gen.ik_solver.solve_single(goal_pose, start_state.position.view(1,-1), start_state.position.view(1, 1, -1))
print(f"result.status:{result.status} ik_result.success:{ik_result.success}")
# result.status: Opt Fail ik_result.success:tensor([[True]], device='cuda:0')

I visualized the robot spheres and did not find any collision. Any suggestions to debug it further, please? Thanks a lot.

[balakumar-s]

We have a tool collision sphere that is offset from the last link by 5 cm. Since you don't have any gripper at the end,
Can you remove 'tool0' from collision_link_names here https://github.com/NVlabs/curobo/blob/main/src/curobo/content/configs/robot/ur5e.yml#L30 and see if that fixes your issue?

We will remove this sphere in a future release.

[ravijo]

@balakumar-s

Lovely. It works like a charm. Thank you so much. Surprisingly, I noticed that result.status is None but robot moves inside Isaac Sim. What's going on?

result.status: None ik_result.success:True

If you don't mind, may I ask a couple of questions?

• How do you compute the distance between a robot and objects in the world to find a collision?
• Instead of having a start and goal, how do you add/provide via-points to the trajectory?

Once again, thank you for this awesome work.

[balakumar-s]

Regarding collision computation, here is an overview: https://curobo.org/source/getting_started/2c_world_collision.html . Essentially we have CUDA implementations of signed distance functions for different world representations.

Currently there is no way to provide via points, the closest we have is approach vector offset planning: https://curobo.org/source/advanced_examples/3_constrained_planning.html#grasp-approach-vector