Failing a 'base case' motion generation planning problem

[jstmn]

1. cuRobo installation mode: python
2. python version: 3.8
3. Isaac Sim version (if using): n/a

---

Issue Details

Hi,

It seems like MotionGen.plan_single() is failing to generate plans for 'base case' planning problems. Specifically, for the 'iiwa_allegro.yml' robot model i'm trying to generate a motion plan between the all 0 config, to the target state found by running FK(q0'), where q0' = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0, ..., 0]

edit: to clarify q0' is a joint configuration of the robot. In this case it is all 0.1 for the iiwa, and 0 for the allegro. FK(q0) is the target end effector pose

Here is a minimum working example of the issue. result.success is always false.

import torch
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

device = "cuda:0"
robot_file = "iiwa_allegro.yml"
tensor_args = TensorDeviceType(device=device, dtype=torch.float32)
world_config = {
    "cuboid": {
        "dummy": {"dims": [0.1, 0.1, 0.1], "pose": [100.0, 0.0, 0.0, 1, 0, 0, 0]},
    },
    "mesh": {},
}
motion_gen_config = MotionGenConfig.load_from_robot_config(
    robot_file,
    world_config,
    tensor_args,
    interpolation_dt=0.01,
)

motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(enable_graph=True)
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)

# Get the goal pose.
q0_adjusted = torch.zeros((1, 23), dtype=torch.float32, device=device)
q0_adjusted[0, 0:7] += 0.1
q0_adjusted_state = motion_gen.rollout_fn.compute_kinematics(JointState.from_position(q0_adjusted))
goal_pose = Pose(q0_adjusted_state.ee_pos_seq.clone(), quaternion=q0_adjusted_state.ee_quat_seq.clone())

# Get the start state.
q0 = torch.zeros((1, 23), dtype=torch.float32, device=device)
start_state = JointState.from_position(q0)

result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
print("success: ", result.success)
if result.success:
    print(result.optimized_plan.position.shape)
traj = result.get_interpolated_plan()

Any ideas?

Thanks

• jeremy

[blooop]

I have been having similar issues but had not got around to making a minimal example. My case is slightly different in that I am planning in joint space, but the other aspects of the problem are very similar.

I was trying to plan a case where the arm started out vertical, and ended up in a horizonal state. I was not able to get the franka to plan from the default starting state to fully vertical, so I had to add two intermediate states in order to be able to solve the plan semi-reliably (it will still often fail with this approach).

I've modified the minimal example from jstmn to match the problem I was having. In this minimal example my workaround of moving to intermediate states doesn't seem to work, and it fails for intermediate plans

from typing import List
from math import radians
from itertools import pairwise

import torch
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util_file import get_robot_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig

device = "cuda:0"
robot_file = "franka.yml"
tensor_args = TensorDeviceType(device=device, dtype=torch.float32)
world_config = {
    "cuboid": {
        "dummy": {"dims": [0.1, 0.1, 0.1], "pose": [100.0, 0.0, 0.0, 1, 0, 0, 0]},
    },
    "mesh": {},
}
motion_gen_config = MotionGenConfig.load_from_robot_config(
    robot_file,
    world_config,
    tensor_args,
    interpolation_dt=0.01,
)

motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(enable_graph=True)
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)

def to_js(joint_state: List) -> JointState:
    return JointState.from_position(
        torch.Tensor(joint_state).view(1, -1).to(device),
    )

franak_default_js = [0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0]
step_by_step = [
    [0, radians(0), 0, 0, 0, radians(90), 0],
    [0, radians(0), 0, 0, 0, radians(140), 0],
    [0, radians(0), 0, 0, 0, radians(180), 0],
    [0, radians(90), 0, 0, 0, radians(180), 0],  
]

# Try planning from the default state to all the intermediate states.  Only the first one will succeed
for js in step_by_step:
    result = motion_gen.plan_single_js(to_js(franak_default_js), to_js(js))
    print(f"{result.success.cpu()[0]}: planning directly from default state to {js} ")

# In my own code I found a workaround of moving step by step in smaller plans to get to the desired pose.  
# Here in this minimal example that approach is not working.  In my code I get True for all results, 
# here only the first one succeeds.

# combine the default state with the intermediate steps
step_by_step = [franak_default_js] + step_by_step

print("Step by step")
# Try moving between adjacent step by step points.
for js1, js2 in pairwise(step_by_step):
    result = motion_gen.plan_single_js(to_js(js1), to_js(js2))
    print(f"{result.success.cpu()[0]}: planning from {js1} to {js2}")

Output on my machine:

True: planning directly from default state to [0, 0.0, 0, 0, 0, 1.5707963267948966, 0] 
False: planning directly from default state to [0, 0.0, 0, 0, 0, 2.443460952792061, 0] 
False: planning directly from default state to [0, 0.0, 0, 0, 0, 3.141592653589793, 0] 
False: planning directly from default state to [0, 1.5707963267948966, 0, 0, 0, 3.141592653589793, 0] 
Step by step
True: planning from [0.0, -1.3, 0.0, -2.5, 0.0, 1.0, 0.0] to [0, 0.0, 0, 0, 0, 1.5707963267948966, 0]
False: planning from [0, 0.0, 0, 0, 0, 1.5707963267948966, 0] to [0, 0.0, 0, 0, 0, 2.443460952792061, 0]
False: planning from [0, 0.0, 0, 0, 0, 2.443460952792061, 0] to [0, 0.0, 0, 0, 0, 3.141592653589793, 0]
False: planning from [0, 0.0, 0, 0, 0, 3.141592653589793, 0] to [0, 1.5707963267948966, 0, 0, 0, 3.141592653589793, 0]

Sometimes I get different output, where they all fail, or random ones that normally fail will succeed.

These are what the intermediate states look like:

[jstmn]

hey this is unrelated but @blooop what visualizer/simulator is this?

[blooop]

It's pybullet. Lightweight and easy to use but bad graphics and poor performance if you want to do image/depth capture.

[balakumar-s]

@jstmn For iiwa_allegro.yml, few issues:

1. The allegro joints are out of bounds at 0. You need to change them to be within bounds start and goal.
2. In addition, the config file for iiwa_allegro also requires target poses for fingertips. If you don't want to plan for them, remove make link_names: null here: https://github.com/NVlabs/curobo/blob/3f1beb991fa0a9d84a934f6600a70e854d1db4dd/src/curobo/content/configs/robot/iiwa_allegro.yml#L55
3. If you want to plan for fingertips as well, then add link_poses to planning query.

   plan_single(...., link_poses=q0_adjusted_state.link_pose)

[jstmn]

It's working now! Thanks for the help.

A warning when joint limits are exceeded for the start configuration would be great to add - does this repository accept PRs? I can add this if so.

[balakumar-s]

A warning is already published for invalid start state. However this requires the logger level to be set correctly.

Set setup_logger('warning') before calling cuRobo's planning functions will print it. https://curobo.org/_api/curobo.util.logger.html#curobo.util.logger.setup_logger

We are working on improving failure debugging. Until then you can use https://curobo.org/_api/curobo.wrap.reacher.motion_gen.html#curobo.wrap.reacher.motion_gen.MotionGenResult.valid_query to see if the planning problem is valid.

[jstmn]

It looks like neither of these are working actually. I tried debug mode for the logger and didn't see anything either

from curobo.util.logger import setup_curobo_logger
setup_curobo_logger('warning')
...
q0 = torch.zeros((1, 23), dtype=torch.float32, device=device)
q0[:, -4] = -0.3  # thumb link 0 has range [0.279244444444, 1.57075]
...
result = motion_gen.plan_single(start_state, goal_pose, MotionGenPlanConfig(max_attempts=1))
print("valid_query:", result.valid_query)

output:

valid_query: True
success:     tensor([False], device='cuda:0')

[balakumar-s]

valid_query is only set to False when the graph planner is invoked. In your case, IK fails so graph planner is never invoked. The only information you would get is from result.status which would have IK Fail.

[balakumar-s]

Added start state validity check to motion planning queries.