In this paper, we present a method to initialize at a feasible point and unfailingly solve a non-convex optimization problem in which a set-point motion is planned for a multi-link manipulator under state and control constraints. We construct an initial feasible solution by analyzing the final time effect for feasibility problems of flatness based motion planning problems. More specifically, we first find a feasible time-optimal trajectory under state constraints without a control constraint by solving a linear programming problem. Then, we find a feasible trajectory under control constraints by scaling the trajectory. To evaluate the practical applicability of the proposed method, we did numerical experiments to solve a multi-link manipulator motion planning problem by combining the method with recursive inverse dynamics algorithms.
Although the dynamic equations of motion of open-chained robot systems are well known, they are seldom taken into account during the planning of motions. In this work, we show that the dynamics of a robot can be used to produce motions that extend the payload capability beyond the limit set by traditional methods. In particular, we develop a point-to-point weightlifting motion planner for open-chained robots. The governing optimal control problem is converted into a direct, SQP parameter optimization in which the gradient is determined analytically. The joint trajectories are defined by B-spline polynomials along with a time-scale factor. The algorithm is applied to a Puma 762 robot, with its physical limitations incorporated into the formulation. The torque limits are formulated as soft constraints added into the objective function while the position and velocity limits are formulated as hard, linear inequality constraints, on the parameters. The solutions obtained with our algorithm extend the robot's payload capability while reducing the joint torques. Interestingly, nearly all the trajectories found pass through singular configurations, where large internal forces from the robot are applied to the payload and little torque is needed from the motors.