Analysis and acceleration of TORM: optimization-based planning for path-wise inverse kinematics

A redundant manipulator can have many trajectories for joints that follow a given end-effector path in the Cartesian space, since it has multiple inverse kinematics solutions per end-effector pose. While maintaining accuracy with the given end-effector path, it is challenging to quickly synthesize a feasible trajectory that satisfies robot-specific constraints and is collision-free against obstacles, especially when the given end-effector path passes around obstacles. In this paper, we present a trajectory optimization of a redundant manipulator to synthesize a trajectory that follows a given end-effector path accurately, while achieving smoothness and collision-free manipulation. Our method holistically incorporates three desired properties into the trajectory optimization process by integrating the Jacobian-based inverse kinematics solving method and an optimization-based motion planning approach. Specifically, we optimize a trajectory using two-stage gradient descent to reduce potential competition between different properties during the update. To avoid falling into local minima, we iteratively explore different candidate trajectories with our local update. We also accelerate our optimizer by adaptively determining the stop of the current exploration based on the observation of optimization results. We compare our method with five prior methods in test scenes, including external obstacles and two non-obstacle problems. Furthermore, we analyze our optimizer performance by experimenting with three different configurations of robots. Our method robustly minimizes the pose error in a progressive manner while satisfying various desirable properties.