This article proposes an efficient and probabilistic complete planning algorithm to address motion planning problem involving orientation constraints for decoupled dual-arm robots. The algorithm is to combine sampling-based planning method with analytical inverse kinematic calculation, which randomly samples constraint-satisfying configurations on the constraint manifold using the analytical inverse kinematic solver and incrementally connects them to the motion paths in joint space. As the analytical inverse kinematic solver is applied to calculate constraint-satisfying joint configurations, the proposed algorithm is characterized by its efficiency and accuracy. We have demonstrated the effectiveness of our approach on the Willow Garage’s PR2 simulation platform by generating trajectory across a wide range of orientation-constrained scenarios for dual-arm manipulation.
Combined Task and Motion Planning for a Dual-arm Robot to Use a Suction Cup Tool