# Feasibility Retargeting for Multi-contact Teleoperation and Physical Interaction

Quentin Rouxel, Ruoshi Wen, Zhibin Li, Carlo Tiseo, Jean-Baptiste Mouret, Serena Ivaldi

► To cite this version:

Quentin Rouxel, Ruoshi Wen, Zhibin Li, Carlo Tiseo, Jean-Baptiste Mouret, et al. Feasibility Retargeting for Multi-contact Teleoperation and Physical Interaction. 2nd Workshop Toward Robot Avatars, 2023 IEEE International Conference on Robotics and Automation (ICRA), Jun 2023, London, United Kingdom. 10.1109/tmech.2022.3152844/mm2. [hal-04161472](https://hal.science/hal-04161472)

HAL Id: hal-04161472

https://hal.science/hal-04161472

Submitted on 3 Aug 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Feasibility Retargeting for Multi-contact Teleoperation and Physical Interaction

Quentin Rouxel*, Ruoshi Wen†, Zhibin Li‡, Carlo Tiseo§, Jean-Baptiste Mouret*, Serena Ivaldi*

*Inria, CNRS, Université de Lorraine, France

†Institute for Perception, Action, and Behaviour, School of Informatics, University of Edinburgh, UK

‡Department of Computer Science, University College London, UK

§School of Engineering and Informatics, University of Sussex, UK

*Abstract*—This short paper outlines two recent works [1, 2] on multi-contact teleoperation and the development of the SEIKO (Sequential Equilibrium Inverse Kinematic Optimization) framework. SEIKO adapts commands from the operator in real-time and ensures that the reference configuration sent to the underlying controller is feasible. Additionally, an admittance scheme is used to implement physical interaction, which is then combined with the operator's command and retargeted. SEIKO has been applied in simulations on various robots, including humanoid and quadruped robots designed for loco-manipulation. Furthermore, SEIKO has been tested on real hardware for bimanual heavy object carrying tasks.

## I. INTRODUCTION

As avatars become more capable and human-like, safety concerns arise due to the risk of falls and harm to the robot or its environment from operator errors [3]. The complexity of understanding the robot's balance and kinematics makes it necessary to automatically filter and retarget commands to ensure safe and feasible movements. Constraints include balance, joint position limits, stability conditions [4] for contact points (i.e. no pulling, no slipping, and no tilting), actuator torque limits. Our approach ensures that the desired configuration sent to the low-level controller always meets these feasibility constraints, enhancing safety and reducing the risk of harm.

Humans rely on contacts with their hands and feet for daily tasks. It enhances stability, increases the reaching distance for objects that are further away, and allows for a greater maximum pushing force when performing tasks such as opening doors. However, achieving balance and stability on uneven surfaces and with multiple contacts poses unique challenges [5–10]. The kinematic-only COM projection and ZMP criteria used for flat ground balance is insufficient [11, 12], as contact forces have infinite solutions, and the robot's force distribution can change with and without movement. To ensure stability, we consider the mutual influence of posture and contact force distribution while enforcing the appropriate constraints.

Previous works that studied multi-contact and humanoid teleoperation [13] primarily focused on contact surfaces co-planar to the ground, whereas this work addresses the more general case of uneven surfaces and emphasizes smooth contact switching and feasibility boundary enforcement. While similar nonlinear optimizations were used for offline planning [8] and posture generation [10], this work specifically

<figure><img src="image_2.png" /><figcaption>Figure 1. Multi-contact configurations computed by SEIKO during teleoperated tasks on Valkyrie and Talos humanoid robots, on ANYmal quadruped robot and on dual arms bimanual Franka robot.</figcaption></figure>

prioritizes computational efficiency to enable real-time online reactive teleoperation.

## II. RETARGETING OPTIMIZATION

This short paper summarizes the two works [1] and [2] that developed the SEIKO framework for Sequential Equilibrium Inverse Kinematic Optimization. SEIKO adapts operator commands to satisfy feasibility constraints and resolves posture and contact force redundancies for intuitive and robust avatar command. As depicted in Fig. 1, the method has been applied in simulation and on real hardware on several robots.

SEIKO is a model-based method for multi-contact tasks on robots with multiple limbs and posture redundancy, including both fixed and floating base robots. It ensures the balance of either the robot itself through multiple supporting contacts or of an object being manipulated with multiple grasping points.

SEIKO assumes quasi-static conditions, making the problem instantaneous and avoiding consideration of future nonlinear dynamics. It optimizes nonlinear desired robot kinematics $q^d$ and contact forces $\lambda^d$ using a Sequential Quadratic Programming (SQP) scheme, solving a Quadratic Program (QP) at each time step:
$$
\begin{align}
\min_{\dot{x}} & \quad \|C_{\text{cost}}(x_t)\dot{x} - c_{\text{cost}}(x_t)\|_w^2 \quad \text{s.t.} \nonumber \\
C_{\text{eq}}(x_t)\dot{x} + c_{\text{eq}}(x_t) &= 0, \quad C_{\text{ineq}}(x_t)\dot{x} + c_{\text{ineq}}(x_t) \ge 0 \tag{1} \\
\text{where } x_t &= \begin{bmatrix} q^d \\ \lambda^d \end{bmatrix}, \quad \dot{x} = \begin{bmatrix} \dot{q} \\ \dot{\lambda} \end{bmatrix}, \nonumber
\end{align}
$$

Here, $x_t$ is the current desired configuration, and the incremental change $\dot{x}$ is the decision variable of the QP. At each time step, the QP is solved and the desired configuration is integrated $x_{t+1} = x_t + \dot{x}\Delta t$. The QP optimizes various objectives $C_{\text{cost}}, c_{\text{cost}}$, including target effector poses, joint torques, and contact force minimization, while ensuring feasibility through equality and inequality constraints $C_{\text{eq}}, c_{\text{eq}}, C_{\text{ineq}}, c_{\text{ineq}}$. Pinocchio library [14] is used to compute the analytical partial derivatives of the static equation of motion to formulate $C_{\text{eq}}, c_{\text{eq}}: G(q) = S\tau + J(q)^{\top}\lambda$ where $G(q)$ is the gravity vector, $\tau$ is the joint torque, $J(q)$ is the stacked Jacobian matrix of enabled contacts, and $S$ is the selection matrix accounting for the floating base. The optimization runs in real-time (one iteration is under 1ms), converges quickly and continuously retargets operator commands to feasible desired configurations of posture and contact forces.

SEIKO generates anthropomorphic postures for humanoid
emojis by minimizing joint torques. [15, 16] optimize directly
the position of the COM to maximize robustness margins
against disturbances during multi-contact sliding tasks. In
contrast, our work solves for velocities, focuses on achiev-
ing stability close to the feasibility limits in order to fully
exploit the capabilities of the system and proposes a smooth
contact switching procedure that is critical for complex loco-
manipulation tasks. While SEIKO has successfully accom-
plished teleoperated tasks at slow or moderate speeds, it is not
suitable for fast motions that consider inertial effects. Contact
locations are not optimized, and the operator must select the
position and sequence of contact points.

III. INTERFACE, INTERACTION AND ARCHITECTURE

Designing human-robot interfaces for teleoperated multi-
contact tasks is a complex challenge. Tracking the entire body
of the operator as a reference [17–20] is inconvenient for
humanoid robots due to factors like operator fatigue, kinematic
and inertia mismatch, and the need to replicate environmental
contacts. Our approach, illustrated in Fig. 2, commands the
Cartesian pose of end-effectors and uses buttons to trigger
contact switches.

To perform collaborative tasks such as carrying a large
object or being physically guided, robotic avatars should be
able to interact with humans or their environment. Compliance
at end-effectors is helpful for establishing contacts without cre-
ating disturbances. Our work achieves end-effector compliance
using a velocity admittance scheme computed from external
forces (Fig. 3), enabling a human collaborator to push and
move the robot’s effectors (Fig.4). However, the challenge
is to maintain feasibility constraints and prevent the robot
from being pushed towards an infeasible or dangerous posture

<figure><img src="image_3.png" /><figcaption>Figure 2. User interface for simulated multi-contact teleoperation (left) where the operator commands the pose of free end-effectors and triggers contact switch. Teleoperation interface for remote bimanual docking and assembly task (right)

Figure 3. An admittance controller processes external forces into velocity commands, which are integrated into a relative reference pose. SEIKO adapts the combined teleoperation and physical interaction commands to satisfy system and task constraints, producing feasible desired posture and contact wrenches. The interaction controller realizes the desired configuration using passive impedance and load compensation controllers.</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>Figure 3. An admittance controller processes external forces into velocity commands, which are integrated into a relative reference pose. SEIKO adapts the combined teleoperation and physical interaction commands to satisfy system and task constraints, producing feasible desired posture and contact wrenches. The interaction controller realizes the desired configuration using passive impedance and load compensation controllers.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Figure 4. A stack of books is held and moved through physical interaction by a dual-arm robot and a local operator collaborating together.</figcaption></figure>

by resisting physical interaction when necessary. Our work
demonstrates the implementation of safe physical interaction
while superimposing the operator’s pose commands.

A filtering pipeline including low-pass, maximum velocity,
and acceleration filters, processes effector pose commands
before SEIKO to further enhance robustness against aggressive
motions and network communication jitter.

IV. RESULTS AND CONCLUSION

The SEIKO framework has undergone extensive validation
on humanoid, quadruped, and fixed-base dual-arm robots,
with both plane and point contacts. We have utilized Gazebo
and PyBullet simulators [1] to test multi-contact tasks, such
as extreme reaching beyond feasibility boundaries, pushing,
contact switching, and traversal of complex and uneven terrain.
We presented quantitative analysis showing that despite the
quasi-static assumption, practical tasks with moderate speed
were achievable. On humanoid Valkyrie, simulated hand ve-
locity can reach 30 cm/s by trading off maximum reachable
distance for more conservative postures. The framework has
also been tested on real robots [2] for bimanual grasping and
maneuvering of heavy objects in arbitrary ways, guiding the
robot through physical interactions with stacks of unknown
objects, collaborative part assembly, and insertion of industrial
connectors. These results illustrated in our videos¹² highlight
the versatility and robustness of the SEIKO framework for a
wide range of robotic applications.

<footnote n="1">¹https://doi.org/10.1109/TMECH.2022.3152844/mm2</footnote>

<footnote n="2">²https://youtu.be/A0bjCIIHyjQ</footnote>
## ACKNOWLEDGMENT

This research is supported by the EPSRC Future AI and Robotics for Space (EP/R026092), ORCA (EP/R026173), NCNR (EP/R02572X), EU Horizon2020 project THING (ICT-2017-1), EU Horizon2020 project Harmony (101017008), and EU Horizon project euROBIN (101070596).

## REFERENCES

[1] Q. Rouxel, K. Yuan, R. Wen, and Z. Li, "Multicontact motion retargeting using whole-body optimization of full kinematics and sequential force equilibrium," *IEEE/ASME Transactions on Mechatronics*, pp. 1–11, 2022.

[2] R. Wen, Q. Rouxel, M. Mistry, Z. Li, and C. Tiseo, "Collaborative bimanual manipulation using optimal motion adaptation and interaction control: Retargeting human commands to feasible robot control references," *IEEE Robotics & Automation Magazine*, pp. 2–14, 2023.

[3] M. Johnson, B. Shrewsbury, S. Bertrand, T. Wu, D. Duran, M. Floyd, P. Abeles, D. Stephen, N. Mertins, A. Lesman et al., "Team ihmc's lessons learned from the darpa robotics challenge trials," *Journal of Field Robotics*, vol. 32, no. 2, pp. 192–208, 2015.

[4] S. Caron, Q.-C. Pham, and Y. Nakamura, "Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas," in *2015 IEEE International Conference on Robotics and Automation (ICRA)*, 2015.

[5] L. Sentis, J. Park, and O. Khatib, "Modeling and control of multi-contact centers of pressure and internal forces in humanoid robots," in *2009 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2009, pp. 453–460.

[6] O. Khatib and S.-Y. Chung, "Suprapeds: Humanoid contact-supported locomotion for 3d unstructured environments," in *2014 IEEE International Conference on Robotics and Automation (ICRA)*, 2014, pp. 2319–2325.

[7] A. Di Fava, K. Bouyarmane, K. Chappellet, E. Ruffaldi, and A. Kheddar, "Multi-contact motion retargeting from human to humanoid robot," in *IEEE-RAS 16th International Conference on Humanoid Robots*, 2016, pp. 1081–1086.

[8] R. Shigematsu, M. Murooka, Y. Kakiuchi, K. Okada, and M. Inaba, "Generating a key pose sequence based on kinematics and statics optimization for manipulating a heavy object by a humanoid robot," in *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2019, pp. 3852–3859.

[9] K. Bouyarmane and A. Kheddar, "Humanoid robot locomotion and manipulation step planning," *Advanced Robotics*, vol. 26, no. 10, pp. 1099–1126, 2012.

[10] S. Brossette, A. Escande, and A. Kheddar, "Multicontact postures computation on manifolds," *IEEE Transactions on Robotics*, 2018.

[11] F. Abi-Farraj, B. Henze, A. Werner, M. Panzirsch, C. Ott, and M. Roa, "Humanoid teleoperation using task-relevant

haptic feedback," in *IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, IROS'18*, 2018.

[12] Y. Ishiguro, K. Kojima, F. Sugai, S. Nozawa, Y. Kakiuchi, K. Okada, and M. Inaba, "Bipedal oriented whole body master-slave system for dynamic secured locomotion with LIP safety constraints," in *2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2017, pp. 376–382.

[13] K. Darvish, L. Penco, J. Ramos, R. Cisneros, J. Pratt, E. Yoshida, S. Ivaldi, and D. Pucci, "Teleoperation of humanoid robots: A survey," *IEEE Transactions on Robotics*, pp. 1–22, 2023.

[14] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, "The pinocchio c++ library – a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives," in *IEEE International Symposium on System Integrations (SII)*, 2019.

[15] S. Samadi, S. Caron, A. Tanguy, and A. Kheddar, "Balance of humanoid robots in a mix of fixed and sliding multi-contact scenarios," in *2020 IEEE International Conference on Robotics and Automation (ICRA)*, 2020, pp. 6590–6596.

[16] S. Samadi, J. Roux, A. Tanguy, S. Caron, and A. Kheddar, "Humanoid control under interchangeable fixed and sliding unilateral contacts," *IEEE Robotics and Automation Letters*, vol. 6, no. 2, pp. 4032–4039, 2021.

[17] F.-J. Montecillo-Puente, M. N. Sreenivasa, and J.-P. Laumond, "On real-time whole-body human to humanoid motion transfer," in *ICINCO*, 2010.

[18] J. Koenemann, F. Burget, and M. Bennewitz, "Real-time imitation of human whole-body motions by humanoids," in *2014 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2014, pp. 2806–2812.

[19] L. Penco, B. Clément, V. Modugno, M. Hoffman, G. Nava, D. Pucci, N. Tsagarakis, J.-B. Mouret, and S. Ivaldi, "Robust real-time whole-body motion retargeting from human to humanoid," in *IEEE-RAS Int. Conference on Humanoid Robots (HUMANOIDS)*, 2017.

[20] K. Darvish, Y. Tirupachuri, G. Romualdi, L. Rapetti, D. Ferigo, F. J. A. Chavez, and D. Pucci, "Whole-body geometric retargeting for humanoid robots," in *2019 IEEE-RAS 19th International Conference on Humanoid Robots (Humanoids)*. IEEE, 2019, pp. 679–686.