Robust Execution of Contact-Rich Motion Plans by Hybrid Force-Velocity Control

Abstract: In hybrid force-velocity control, the robot can use velocity control in some directions to follow a trajectory, while performing force control in other directions to maintain contacts with the environment regardless of positional errors. We call this way of executing a trajectory hybrid servoing. We propose an algorithm to compute hybrid force-velocity control actions for hybrid servoing. We quantify the robustness of a control action and make trade-offs between different requirements by formulating the control synthesis as optimization problems. Our method can efficiently compute the dimensions, directions and magnitudes of force and velocity controls. We demonstrated by experiments the effectiveness of our method in several contact-rich manipulation tasks. Link to the video: https://youtu.be/KtSNmvwOenM.

• The paper introduces a framework that optimizes the allocation between force and velocity control to achieve robust contact-rich motion execution.
• It formulates control synthesis as a constrained optimization problem, ensuring alignment with holonomic constraints and enhancing disturbance rejection.
• Experiments on tasks like block tilting and tile levering-up validate the approach's effectiveness in underactuated, multi-contact scenarios.

Robust Hybrid Force-Velocity Control for Contact-Rich Manipulation: Methods, Analysis, and Experimentation

Introduction

The paper "Robust Execution of Contact-Rich Motion Plans by Hybrid Force-Velocity Control" (1903.02715) addresses robust execution strategies for contact-rich robotic manipulation tasks. The authors present a principled framework for hybrid servoing, in which velocity control is employed along selected axes to achieve precise trajectory following, while force control is applied along others to maintain compliance and sustain environmental contacts under uncertainty. Unlike previous approaches that focus on simple or fully actuated systems, this work generalizes hybrid force-velocity control to broader multi-body, underactuated, and high-DOF systems. The formulation casts the hybrid control synthesis as a set of optimization problems, with explicit mechanisms for balancing robustness, constraint satisfaction, and task feasibility.

Problem Formulation and Modeling

Hybrid Servoing Problem

The system is modeled as a set of rigid bodies (robot and objects) with combined configuration $q\in\mathbb{R}^{n_q}$. Generalized velocities $v$ are partitioned into uncontrolled/object DOFs and actuated/robotic DOFs, with associated generalized forces $f$. The execution objective is specified as an affine constraint $Gv=b_G$, representing a desired instantaneous generalized velocity that encodes the motion plan step.

Natural constraints derive from holonomic (persistent contact) relationships and Newton's second law, ensuring task-level feasibility and physical consistency under actuation limits and external forces. Additionally, guard conditions are imposed as affine constraints on force variables to maintain correct contact modes—for instance, friction cone enclosures and normal force thresholds.

The control synthesis seeks:

1. The allocation of DOF into velocity-controlled and force-controlled subspaces (i.e., dimensions and directions).
2. The directions of each control type—the axes in generalized velocity/force space.
3. The magnitudes/commands for each subspace at a given time step.

All constraints must be satisfied, but the allocation and synthesis are optimized to maximize robustness—velocity commands are chosen close to the null space of holonomic constraints, and controls are made as orthogonal and decoupled as possible to improve disturbance rejection.

Figure 1: Relation between velocity commands and holonomic natural constraints. The robot (blue) has a velocity-controlled joint and a force-controlled joint, orthogonal to each other. The table provides a holonomic constraint blocking downward motion. Feasibility depends on alignment between commands and constraints.

Figure 2: Relation between different velocity commands. Two robots apply distinct velocity commands to an object, and feasibility depends on their mutual alignment and contact constraints.

Implications for Robustness

The authors show that full orthogonality between velocity commands and holonomic constraints is not always attainable in underactuated/multibody scenarios, but proximity to this condition correlates with increased robustness. Moreover, velocity controls should be mutually orthogonal to mitigate coupling-induced infeasibility and ensure disturbance resilience.

The selection of velocity commands and force control axes is thus formulated as a constrained optimization problem, where the cost function encourages these alignments and penalizes non-orthogonality and poor null-space alignment.

Hybrid Control Synthesis Algorithm

Velocity-Controlled Subspace Construction

Given the system's natural and task constraints, the minimal number of velocity-controlled axes is determined as $n_{av}^{min} = \text{rank}([N;G]) - \text{rank}(N)$, where $N$ encodes holonomic constraints. This ensures sufficient rank to reduce the system to the task-specified subspace. The maximum, subject to non-overconstraint, is $\text{Dim}(\text{null}(N))$.

The axes are chosen via an optimization problem that seeks (i) linear independence and mutual orthogonality, and (ii) minimal projection onto $N$, i.e., maximal overlap with the holonomic null-space. The solution is computed numerically with projected gradient descent from multiple initial conditions to avoid poor local minima. The cost function weights minimization of cross-projections (off-diagonal of $C$) and maximization of projections onto the null space.

Force-Controlled Subspace and Command Synthesis

With the velocity-controlled axes fixed, the remaining DOFs are assigned to force control. Force commands are solved under the equality constraints (modified Newton's laws and actuation) and inequality constraints (guard conditions, e.g., friction cones) using quadratic programming. The internal force solution that satisfies all constraints with minimal norm is selected for robustness against unmodeled disturbances.

Figure 3: Block tilting example—robot induces rotation about the block edge via point contact during hybrid force-velocity control.

Figure 4: Illustration of coordinate frames relevant to the manipulation scenario.

Application to Contact-Rich Manipulation

Example: Block Tilting Task

The framework is instantiated in the block-tilting manipulation task, where a robot finger rotates a free-standing block about a table edge via point contact. The generalized state includes both the block and finger pose; the task velocity is a twist about the contact axis, and holonomic constraints enforce sticking between block, hand, and table. Guard conditions ensure all three contact forces remain within friction cones and above a normal force threshold.

The algorithm computes that only one velocity-controlled DOF is needed—motion in the desired tilt direction. The orthogonal subspace receives compliant force control (two axes), ensuring the hand complies with block- and table-imposed reactions during tilting regardless of residual errors.

Experimentation and Robustness Quantification

Experiments are performed on block tilting and tile levering-up tasks (pivoting a tile at a box corner). The hybrid control commands are executed on a velocity-controlled manipulator with a force-torque wrist sensor.

Figure 5: Experiment setup for block tilting (left) and tile levering-up (right).

The hybrid controller successfully executes 47 of 50 block tilting runs (each with 15 control steps); failures are predominantly due to controller delays or force tracking errors, not the hybrid allocation itself. Levering-up succeeds in about two-thirds of trials—failure modes include unexpected contact transitions and force-tracking latency.

The primary computational bottleneck is the velocity subspace optimization (35 ms in C++, 250–350 ms in MATLAB), with force QPs solved rapidly. The framework's flexibility is demonstrated via direct application to bottle rotation and other multi-contact tasks.

Figure 6: Schematic for the bottle rotation problem, a complex 6D manipulation scenario addressable with the proposed hybrid control synthesis.

Theoretical and Practical Implications

The authors claim that hybrid servoing, as formulated and automated here, allows for robust and feasible execution of contact-rich plans while minimizing the need for detailed manual engineering or trial-and-error tuning of hybrid allocation. Unlike many previous works, the method handles underactuated, overconstrained, and free-object cases, generalizing beyond robot-to-fixed-environment interactions. The explicit quantification of null-space overlap and orthogonality provides a measure of robustness previously only considered heuristically.

However, the paper acknowledges that the constraint and cost structure remains dependent on intuitive (rather than formally derived) choices, and local minima in the velocity allocation optimization can impact solution quality. Non-holonomic constraints and more sophisticated contact mode reasoning (e.g., automated mode switching or contact transition handling) are open issues not directly addressed by the present method.

Future Directions

Further integration of non-holonomic constraints, improved initialization/optimization for velocity command subspace selection, and principled derivation of robustness metrics from first principles of contact dynamics remain as areas for future development. More efficient approximations or problem structures might be sought to enable real-time use in highly dynamic, multi-fingered or multi-agent settings.

Potential integration with learning-based planners (providing real-time feasible hybrid allocations for learned plans), and formal certification of contact mode maintenance, are both natural extensions.

Conclusion

This work provides a unified and automatable framework for hybrid force-velocity control allocation and execution in contact-rich manipulation, grounded in explicit constraint satisfaction and robustness-oriented optimization over control axes. It demonstrates practical effectiveness on nontrivial manipulation scenarios and generalizes hybrid control synthesis beyond previously considered cases. The methodology stands as a strong basis for further advancement in robust execution of contact-rich and multi-contact robotic manipulation tasks.