SwitchOpt: Trajectory Optimization with Adaptive Grasp Target Switching

We address the problem of trajectory optimiza- tion in scenarios where multiple grasp targets are available for the same object. We introduce SwitchOpt, an adaptive optimization strategy that dynamically switches between grasp targets during trajectory optimization. Instead of committing to a single candidate, SwitchOpt monitors progress step by step using a merit function that captures trajectory quality and constraint satisfaction. A prediction horizon is used to assess whether the current trajectory is likely to improve further, while a minimum-stay mechanism ensures sufficient refinement before considering a switch. Whenever a switch is considered, SwitchOpt reconstructs candidate trajectories by combining the current head with interpolated tails toward alternative grasp targets, and evaluates each of these full trajectories with the same merit function over the prediction horizon. If the best candidate is predicted to outperform continuing toward the current target, that target is selected as the new goal and its reconstructed trajectory is used as the new initialization, allowing the solver to continue from a promising adapted trajectory. This principled selection strategy balances local exploitation of the current target with structured exploration of alternative grasp poses, maintaining optimization continu- ity between switches. Experiments in simulation demonstrate that SwitchOpt improves final trajectory quality, accelerates convergence, and increases feasibility in multi-target trajectory optimization.