plan_single_js() does not quite reach the goal state

[blooop]

1. cuRobo installation mode: python
2. python version: 3.10

I noticed that when I was doing plan_single_js() plans that often the robot would get quite close but not fully reach the desired joint state.

I found a workaround of executing the plan, and then performing another plan with the same goal position. The second plan would get the robot to the desired goal state.

This is a minimal example that reproduces the bug and the workaround based off this demo: https://curobo.org/get_started/2a_python_examples.html#motion-generation

import torch
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig

world_config = {
    "cuboid": {
        "table": {
            "dims": [5.0, 5.0, 0.2],  # x, y, z
            "pose": [100.0, 0.0, -0.1, 1, 0, 0, 0.0],  # x, y, z, qw, qx, qy, qz
        },
    },
}

motion_gen_config = MotionGenConfig.load_from_robot_config(
    "ur5e.yml",
    world_config,
    interpolation_dt=0.01,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()
retract_cfg = motion_gen.get_retract_config()

joint_names = [
    "shoulder_pan_joint",
    "shoulder_lift_joint",
    "elbow_joint",
    "wrist_1_joint",
    "wrist_2_joint",
    "wrist_3_joint",
]

start_state = JointState.from_position(
    torch.zeros(1, 6).cuda(), joint_names=joint_names
)

goal = torch.ones(1, 6) * 0.1
goal_joint_state = JointState.from_position(goal.cuda(), joint_names=joint_names)

# first try does not reach the goal
result = motion_gen.plan_single_js(start_state, goal_joint_state)
final_pos = result.get_interpolated_plan().position[-1]
print(f"final position:         {final_pos}")
print(f"desired final position: {goal_joint_state.position}\n")

# plan to the same goal, from the last point in the trajectory
start_state = JointState.from_position(final_pos.unsqueeze(0), joint_names=joint_names)
result = motion_gen.plan_single_js(start_state, goal_joint_state)
final_pos = result.get_interpolated_plan().position[-1]
print(f"final position:         {final_pos}")
print(f"desired final position: {goal_joint_state.position}")

output:

final position:         tensor([0.0243, 0.0243, 0.0243, 0.0243, 0.0243, 0.0243], device='cuda:0')
desired final position: tensor([[0.1000, 0.1000, 0.1000, 0.1000, 0.1000, 0.1000]], device='cuda:0')

final position:         tensor([0.1000, 0.1000, 0.1000, 0.1000, 0.1000, 0.1000], device='cuda:0')
desired final position: tensor([[0.1000, 0.1000, 0.1000, 0.1000, 0.1000, 0.1000]], device='cuda:0')

[balakumar-s]

We found an issue in the joint space planner that's causing this. We have a fix coming soon (ETA: end of this week). Currently verifying to make sure it doesn't break other things.

[blooop]

Thanks!

[balakumar-s]

This is now fixed in latest commit on main. Can you verify and close?

[blooop]

Yes, it works. thanks for the quick fix!