Whole-body Motion Planning and Control
A unique whole-body control framework that integrates the four parts of our robot is proposed to achieve safe physical human-robot collaboration and expand the workspace. The stack-of-tasks (SoTs) framework is applied to solve the problem of conflicts between the end- effector tasks and the constraints tasks by correctly assigning an order of task priorities and then ensuring that the lower priority tasks are projected into the null space of the higher- priority tasks. We treat the trajectories of the end-effectors as the main task with the top priority, and the joint positions as the sub-task. The main task movements are generated by the high-level planner with the LfD-based method. After receiving the movements and the predefined impedance parameter, the whole-body impedance controller generates the control torque by summing the main task torque and all projected torques of sub-tasks.