Trajectory planning of robots with dynamics and inequalities

Proposes a trajectory planning scheme for open-chain systems to steer in joint space between a start and a goal, while explicitly satisfying the dynamic equations and inequality constraints prescribed in terms of joint torques, joint angles, and their higher derivatives. The algorithm consists of four steps: (i) the structure of the dynamic equations is exploited to embed the dynamic equations explicitly into the constraints; (ii) the inequalities in the space of joint angles and their derivatives are inner approximated by a set of linear inequalities, i.e., a polytope; (iii) a feasible trajectory is then sought within a class of basis functions by using a discrete collocation grid in time where the inequalities are satisfied; and (iv) the feasible trajectories are characterized in terms of a convex set of the coefficients associated with the basis functions. The approach is illustrated in theory and experiments with a master-slave dual-arm manipulation system