Understanding the Failure/Limits of Motion Planning

[Cfather]

Hi, I tried to run a simple motion planning example but curobo failed to find a solution.
So I have a simple world:

cuboid:
  obstacle1:
    dims: [0.01, 4.0, 1.0] # x, y, z
    pose: [0.0, 0.0, 1.5, 0.0, 0.0, 0.0, 1.0] # x, y, z, qx, qy, qz, qw  

It's basically asking the robot to go from one side of the wall to another side of the wall.
You can make the wall higher to make the scenario simpler, or make the wall lower to make it harder.
curobo shows that it failed to find a solution and here's the final solution curobo gives:

I wonder what would be a general way to understand why curobo fails to find a solution, especially when I used a really aggressive setting of parameters.

My code is attached here:

# Third Party
import torch
import scipy.io as sio
import numpy as np
# import pickle

# CuRobo
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import Cuboid, WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.util.logger import setup_curobo_logger
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig

def demo_motion_gen(test_id):
    # Standard Library
    tensor_args = TensorDeviceType()
    world_file = "simple_scenario.yml"

    robot_file = "kinova_gen3.yml"
    motion_gen_config = MotionGenConfig.load_from_robot_config(
        robot_file,
        world_file,
        tensor_args,
        interpolation_dt=0.01,
        # trajopt_dt=0.15,
        # velocity_scale=0.1,
        use_cuda_graph=True,
        # finetune_dt_scale=2.5,
        interpolation_steps=10000,
    )

    motion_gen = MotionGen(motion_gen_config)
    motion_gen.warmup(parallel_finetune=True)

    start_state_tensor = torch.tensor([0, 0.5, 0, -0.5, 0, 0, 0], device='cuda:0')
    goal_state_tensor = torch.tensor([0,-0.5,0, 0.5,0,0, 0], device='cuda:0')
    
    start_state = JointState.from_position(start_state_tensor.view(1, -1))
    goal_state = JointState.from_position(goal_state_tensor.view(1, -1))

    result = motion_gen.plan_single_js(
        start_state,
        goal_state,
        MotionGenPlanConfig(max_attempts=1000, \
                            enable_graph=True, \
                            parallel_finetune=True, \
                            enable_finetune_trajopt=True),
    )

    if_success = result.success.cpu().numpy()
    q = result.optimized_plan.position.cpu().numpy()
    dt = result.optimized_dt.cpu().numpy()
    solve_time = result.solve_time

    print(if_success)
    print(solve_time)

    # save q as a mat file
    result_filename = 'curobo_trajectory.mat'
    sio.savemat(result_filename, \
                {'if_success': if_success, \
                 'q': q, \
                 'dt': dt, \
                 'solve_time': solve_time, \
                 'start_state': start_state_tensor.cpu().numpy(), \
                 'goal_state': goal_state_tensor.cpu().numpy()})

if __name__ == "__main__":
    demo_motion_gen(test_id=0)

[scharalambous-sony]

Hi, Can you post the output from your terminal?
Some things to try:

• Have you verified whether both the start and end state are valid? get_collision_distance will be useful
• Parameters to fine-tune: enable_graph to True and max_attempts to > 1 in MotionGenPlanConfig, and experiment with num_trajopt_seeds, num_graph_seeds and trajopt_tsteps in MotionGenConfig

[Cfather]

@scharalambous-sony Thanks for your reply.

1. Yes, both the start and the goal are feasible.
2. Are there any intuitive explanations for the meaning of num_trajopt_seeds, num_graph_seeds? I tried the suggested values (num_trajopt_seeds = 12, num_graph_seeds = 4) but still did not find a solution.

[Cfather]

Just found that the order of the orientation that I set is wrong.
It is supposed to be "wxyz", rather than "xyzw".