The simultaneous accomplishment of hybrid motion-force control at the end-effector (yellow and magenta colored arrows) and safe handling of unexpected contact caused by human interaction (blue colored arrow).

Scenario #2: Hybrid motion-force control under multi-contact (DLR-KUKA LWR IV+).

Challenges

• Unknown multiple contacts: MPC-based methods for interaction tasks are valid based on a common assumption that the contact occurs at a single and known location, typically at the end-effector where a force/torque (F/T) sensor is installed. In an unstructured workspace, such as scenarios that involve physical interaction with humans or with unknown obstacles, unexpected contact may occur at multiple locations on the robot. The safe handling of multiple contacts has been demonstrated using tactile sensors; however, it is not straightforward how to handle unknown multiple contacts occurring outside the tactile sensor.
• Real-time control without simplifying assumptions: simplifying assumptions on the robot model are common in the literature; e.g., some of the time derivatives are assumed to be constant over the prediction horizon, a quasi-static robot or a linearized robot model is employed, or the robot joints are assumed to be perfectly velocity or position controlled.

Key idea: explicit multi-contact feedback loop in the MPC

This work proposes a contact-feedback MPC framework that addresses complex interaction tasks in which multiple contacts may occur at arbitrary locations. To the best of our knowledge, this is the first attempt that embeds multiple unknown contacts in the MPC framework.

• By making an explicit contact feedback loop using MCP-EP, force control can be accomplished even in scenarios with multiple contacts regardless of contact locations.
• By employing the Differential Dynamic Programming (DDP) algorithm, the proposed MPC operates in real-time for a 7-DOF manipulator without any simplifying assumptions on the robot model.

$\boldsymbol{\tau}_j$ and $\mathbf{W}_b$ are the JTSs and base F/T sensor measurements, respectively. Utilizing the robot states ($\tilde{\mathbf{x}}$) and contact information ($\tilde{\mathbf{r}}_{c,i}$ and $\tilde{\boldsymbol{\lambda}}_i$), the contact-feedback MPC calculates $\mathbf{X}^*$ and $\mathbf{U}^*$. Then, $\mathbf{u}^{[0]*}$ is commanded to the torque-controlled robot.

The multi-contact feedback is provided through MCP-EP, which estimates multiple contact locations and forces using an F/T sensor mounted at the base and JTSs. The proposed MPC does not strictly rely on MCP-EP, while one can employ any method as long as multi-contact information is provided in real time. Since the contact-feedback MPC, MCP-EP, and the low-level torque controller (3 kHz) operate at different rates, each program is run with the latest available data.

Technical components

1. Spring contact model

Left: ${}^{\{C_i\}}\mathbf{r}_{env,i}$ is computed within the contact frame $\{C_i\}$, whose origin coincides with $\tilde{\mathbf{r}}_{c,i}$ and whose z-axis aligns with $\tilde{\boldsymbol{\lambda}}_i$. Middle: $\mathbf{r}_{env,i}$ and $\mathbf{K}_{env,i}$ are calculated by transforming coordinates from $\{C_i\}$ to $\{W\}$. Right: $\mathbf{r}_{c,i}$ is fixed on the robot surface, and the predictive contact force is derived from $\boldsymbol{\lambda}_i = g(\mathbf{x}; \boldsymbol{\theta}_i)$.

Since only z-directional stiffness exists, any movement of the contact point within the xy-plane of the contact frame does not change the force. This implies that an unconstrained motion can be achieved in the tangential direction of the contact force. The contact model does not depend on the control input, which allows the MPC framework to incorporate contact force feedback by avoiding an algebraic loop between $\boldsymbol{\lambda}_i$ and $\mathbf{u}$.

2. Optimal control problem and cost function design

The MPC iteratively solves a discretized optimal control problem (OCP) with the contact-involved robot dynamics:

The running and terminal costs consist of the costs for motion control and contact force control, where the cost for contact is designed to allow for defining different tasks on different links:

• Motion control: regulates the end-effector to the desired position and orientation, with a joint velocity penalization term.
• Contact force regulation: regulates the $i$th contact force to the desired value $\boldsymbol{\lambda}_{i,des}$ in the direction determined by $\mathbf{A}_d$.
• Force barrier: for safe handling of unexpected contacts, the allowed contact force is limited by a force barrier with a quadratic function, $c_\lambda(\|\boldsymbol{\lambda}_i\| - \lambda_{i,max})^2$, which is activated if $\|\boldsymbol{\lambda}_i\| > b\,\lambda_{i,max}$ with $b \in (0, 1)$.

3. Numerical solution of the OCP

The OCP is solved with the Differential Dynamic Programming (DDP) algorithm, specifically Box-FDDP, which allows box-constraints on the control input. The joint angle and velocity limits are handled by a penalization approach, and the partial derivatives of the cost functions and the contact-involved robot dynamics are derived analytically, e.g., $\partial\boldsymbol{\lambda}_i / \partial\mathbf{q} = -\mathbf{K}_{env,i}\mathbf{J}_i(\mathbf{q}, \mathbf{r}_{c,i})$. As a result, the MPC operates in real-time for a 7-DOF manipulator without any simplifying assumptions on the robot model. A planning horizon of 125 ms ($T = 5$, $\Delta t = 25$ ms) and $k_{env,i} = 3500$ N/m are used in all experiments.

Results

The proposed multi-contact feedback MPC is validated in experiments on the DLR-KUKA LWR IV+ torque-controlled robot, with an ATI Omega F/T sensor mounted at the base. There is no pre-knowledge about the contacts; instead, the proposed MPC handles the unexpected contacts in real-time.

Scenario #1: Safe handling of unexpected contact (with and without contact feedback)

Scenario #2: Hybrid motion-force control under multi-contact

(a) The initial configuration of the robot, not in contact. (b) The robot moves in the x-direction and makes contact with the environment. (c) Hybrid motion-force control is performed at the end-effector. (d) While performing hybrid motion-force control, an additional contact is applied by a human.

The contact force in the x-direction is regulated to 20 N while tracking a circular trajectory in the yz-plane, and the contact force on the other links is limited to 15 N.

Additional experiments (not shown in the paper)

Full video

BibTeX

@inproceedings{han2024online,
  title={Online Multi-Contact Feedback Model Predictive Control for Interactive Robotic Tasks},
  author={Han, Seo Wook and Iskandar, Maged and Lee, Jinoh and Kim, Min Jun},
  booktitle={IEEE International Conference on Robotics and Automation (ICRA)},
  pages={11556--11562},
  year={2024}
}