problem of using PoseCostMetric

[frankchan12138]

I tried the plan_single function with PoseCostMetric link the following code（mainly according to simple_stacking.py sample code）.

motion_gen_config = MotionGenConfig.load_from_robot_config(
    robot_dict,
    world_file,
    tensor_args,
    # interpolation_dt=interpolation_dt,
    collision_cache={
        'obb': collision_cache_cuboid,
        'mesh': collision_cache_mesh,
    },
    collision_checker_type=CollisionCheckerType.VOXEL,
    ee_link_name=self.__tool_frame,
    trajopt_tsteps=32,
    use_cuda_graph=True,
    interpolation_dt=0.01
)
......
motion_gen_result = self.motion_gen.plan_single(
    start_state,
    goal_pose,
    MotionGenPlanConfig(
        enable_graph=False,
        max_attempts=10,
        enable_graph_attempt=None,
        enable_finetune_trajopt=True,
        partial_ik_opt=False,
        parallel_finetune=True,
        time_dilation_factor=0.75
        pose_cost_metric = PoseCostMetric.create_grasp_approach_metric(
            offset_position=0.1, tstep_fraction=0.8
        )                        
    )
)

I got the error MotionGenStatus.DT_EXCEPTION, and cannot fix it. Later I found that in the inner code of _plan_from_solve_state funtion, seed_traj will become all zero after _solve_trajopt_from_solve_state.

traj_result = self._solve_trajopt_from_solve_state(
      goal,
      solve_state,
      seed_traj,
      trajopt_instance=self.finetune_trajopt_solver,
      num_seeds_override=seed_override,
      newton_iters=newton_iters,
  )
  finetune_time += traj_result.solve_time

But when I play the simple_stacking.py example， It won't produce result like this. Is there any advice or tips to find out what is wrong? Thanks for all the useful information!!

[frankchan12138]

Hi! @balakumar-s I have another question， I found that when I successfully call plan_single with pose_cost_metric set. The first call will return the result as I want link this

But the second call will return Unexpected result link this

I wonder if there is a way to make the function return result like the first one all the time. Can I get this by tuning parameters?

Thanks!

[balakumar-s]

You can make it deterministic by calling motion_gen.reset(reset_seed=True) between planning calls.

[frankchan12138]

@balakumar-s Thanks for reply， I try to use motion_gen.reset(reset_seed=True) as follow, but still get the same result. i.e first call I get the result I want, but second call will get the unexpected result.

test_pose_cost_metric = PoseCostMetric.create_grasp_approach_metric()
self.motion_gen.reset(reset_seed=True)
motion_gen_result = self.motion_gen.plan_single(
    start_state,
    goal_pose,
    MotionGenPlanConfig(
        enable_graph=False,
        max_attempts=10,
        enable_graph_attempt=None,
        enable_finetune_trajopt=True,
        partial_ik_opt=False,
        parallel_finetune=True,
        time_dilation_factor=time_dilation_factor,
        pose_cost_metric = test_pose_cost_metric
    ),                    
    link_poses = test_link_pose
)

Is that smothing wrong with my code? Thanks for your advice!

[balakumar-s]

I think this is because the optimization problem is doing the following:

1. For first 75% of the trajectory, the robot can move in any direction
2. For remaining 25% of the trajectory, robot is only allowed to move along z direction of the gripper.

Both solutions you get are valid because in the second case, the robot reaches the end-point within the first 75%.

We need an improved cost formulation to fix this. Alternatively, you can use plan_grasp which splits the problem into two separate problems.

[frankchan12138]

@balakumar-s Thanks for reply！ I will try plan_grasplatter.