Fast reaching in clutter while regulating forces using model predictive control

Moving a robot arm quickly in cluttered and unmodeled workspaces can be difficult because of the inherent risk of high impact forces. Additionally, compliance by itself is not enough to limit contact forces due to multi-contact phenomena (jamming, etc.). The work in this paper extends our previous research on manipulation in cluttered environments by explicitly modeling robot arm dynamics and using model predictive control (MPC) with whole-arm tactile sensing to improve the speed and force control. We first derive discrete-time dynamic equations of motion that we use for MPC. Then we formulate a multi-time step model predictive controller that uses this dynamic model. These changes allow us to control contact forces while increasing overall end effector speed. We also describe a constraint that regulates joint velocities in order to mitigate unexpected impact forces while reaching to a goal. We present results using tests from a simulated three link planar arm that is representative of the kinematics and mass of an average male's torso, shoulder and elbow joints reaching in high and low clutter scenarios. These results show that our controller allows the arm to reach a goal up to twice as fast as our previous work, while still controlling the contact forces to be near a user-defined threshold.