Pose-Constrained Path Optimization for Manipulators with Contact Feedback from Sparse Waypoints

Conventional robot motion teaching methods are time-consuming and place a significant burden on operators, especially when trajectory modification is required after contact with the environment. This paper proposes a six-dimensional automatic path correction method that considers both the position and orientation of the end-effector. The operator specifies only the start, goal, and via points of the motion path. Contact wrench information obtained during execution is incorporated into a cost function, and the control points of a spline-based trajectory are iteratively optimized. The method does not rely on an explicit environment model and autonomously updates the trajectory based on contact information. Simulation experiments were conducted on three industrial tasks: part box unloading, yarn cone removal, and part extraction. In all tasks, the proposed method successfully generated collision-free trajectories within a small number of correction iterations and with practical computation time. These results indicate that the proposed approach enables stable and efficient trajectory correction while reducing the teaching burden in industrial support tasks.