Trajectory Optimization for Manipulation Considering Grasp Selection and Adjustment
Jun Shao,Jianfeng Liao,Shiqiang Zhu,Haoyang Zhang,Wei Song
DOI: https://doi.org/10.1007/s10846-023-01946-8
2023-01-01
Journal of Intelligent & Robotic Systems
Abstract:Generating motions to grasp the objects in the scene is critical in robotic manipulation. The feasibility and quality of the output trajectory are closely related to both the grasping pose and the motion to reach the object, and decoupling grasp and motion planning cannot balance these two factors. In this paper, a motion planning method is proposed that comprehensively considers trajectory optimization, trajectory goal selection, and grasp adjustment. Trajectory optimization is formulated as an equality-constrained optimization problem, in which the end configuration of the trajectory can be changed during optimization. The update rule is derived and acts as a basic rule for goal selection and adjustment. The grasp configuration is chosen from a goal set, which helps the optimization to jump out of the local minima and generate smooth and feasible trajectories. We propose a two-stage grasp selection algorithm to balance between exploitation of the trajectory optimization and exploration of the goal set. To further optimize the trajectory, we propose a grasp adjustment method that allows the grasp pose to rotate about the connection line of the contact points in the workspace. To address this complicated problem, we divide the problem into two subproblems: computing the angle to rotate in the workspace and calculating the grasp configuration in the joint space. We design the overall planner and incorporate our method into a complete robotic arm platform with perception and the grasp generation network. All proposed methods are validated by performing comparative simulations and experiments.