  Force modulation of robotic manipulators has been extensively studied for
several decades. However, it is not yet commonly used in safety-critical
applications due to a lack of accurate interaction contact modeling and weak
performance guarantees - a large proportion of them concerning the modulation
of interaction forces. This study presents a high-level framework for
simultaneous trajectory optimization and force control of the interaction
between a manipulator and soft environments, which is prone to external
disturbances. Sliding friction and normal contact force are taken into account.
The dynamics of the soft contact model and the manipulator are simultaneously
incorporated in a trajectory optimizer to generate desired motion and force
profiles. A constrained optimization framework based on Alternative Direction
Method of Multipliers (ADMM) has been employed to efficiently generate
real-time optimal control inputs and high-dimensional state trajectories in a
Model Predictive Control fashion. Experimental validation of the model
performance is conducted on a soft substrate with known material properties
using a Cartesian space force control mode. Results show a comparison of ground
truth and real-time model-based contact force and motion tracking for multiple
Cartesian motions in the valid range of the friction model. It is shown that a
contact model-based motion planner can compensate for frictional forces and
motion disturbances and improve the overall motion and force tracking accuracy.
The proposed high-level planner has the potential to facilitate the automation
of medical tasks involving the manipulation of compliant, delicate, and
deformable tissues.
