Optimal Hybrid Force-Velocity Control

• Optimal hybrid force-velocity control is a technique that splits robot actuation into force and velocity modes for reliable trajectory tracking and contact maintenance under quasi-static assumptions.
• It uses both iterative optimization and closed-form analytical methods to design mutually orthogonal control axes, enhancing system robustness and kinematic conditioning.
• Experimental validations through simulations and real-robot tests demonstrate significantly reduced computation times and improved success rates in contact-rich tasks.

Optimal hybrid force-velocity control (HFVC) is a control paradigm used in robotics for executing contact-rich motion plans in which the control authority is optimally split between force and velocity commands in separate subspaces of the robot’s actuated degrees of freedom (DOFs). The goal is to robustly achieve desired trajectories despite contact constraints, under the quasi-static assumptions of rigid-body mechanics and holonomic contacts. This approach enables simultaneous trajectory tracking and maintenance of specified contact modes (such as sticking), optimizing the allocation of force and velocity control to minimize risk of constraint violation and actuation ill-conditioning. Two major contributions in this area are the robust optimization-based hybrid servoing synthesis (Hou et al., 2019) and the analytically derived closed-form solution for optimal HFVC (Hou et al., 2020).

1. System Model and Problem Definition

HFVC is formulated for a mechanical system comprising a multi-DOF manipulator (actuated part), one or more free rigid objects (unactuated part), and a rigid environment. The generalized coordinates are split as $q \in \mathbb{R}^{n_q}$, and velocities as $v = [v_u^\top, v_a^\top]^\top \in \mathbb{R}^n$, where $v_u$ are unactuated (“free”) velocities (such as object twist), $v_a$ are actuated (robot) velocities, and $n = n_u + n_a$. Forces are $f = [f_u^\top, f_a^\top]^\top$, with $f_u = 0$. The system operates under quasi-static assumptions (neglecting inertia and Coriolis terms).

Contact and motion constraints are modeled as:

• Holonomic/contact constraints: $J\,v = 0$
• Desired trajectory tracking: $G\,v = b_G$

The robot’s actuated subspace $\mathbb{R}^{n_a}$ is decomposed into force-controlled and velocity-controlled directions. Under a suitable transform $T = \operatorname{blockdiag}(I_{n_u}, R_a)$, the coordinates are $$w = T v = [w_u^\top, w_{af}^\top, w_{av}^\top]^\top$$ and applied forces are $$\eta = T f = [\eta_u^\top, \eta_{af}^\top, \eta_{av}^\top]^\top$$, with $w_{av}$ (velocity-controlled) for trajectory execution and $\eta_{af}$ (force-controlled) to enforce contact constraints. The core decision variables are:

• Number and directions of force and velocity axes: $n_{af}$, $n_{av}$, $R_a$
• Command magnitudes: $w_{av}$, $\eta_{af}$

2. Mathematical Formulation and Constraints

The HFVC problem is specified by the following elements:

A. Physical and Contact Constraints

• Holonomic constraints: $J v = 0$
• Static equilibrium (Newton’s law): ${J'}^\top \lambda + f + F_{ext} = 0$
  • Here, $\lambda$ are Lagrange multipliers (contact forces), $f$ are generalized forces, and $F_{ext}$ denotes external forces such as gravity.
• Consistency with friction and guard constraints:
  • $\Lambda [\lambda; f] \le b_\Lambda$,
  • $\Gamma [\lambda; f] = b_\Gamma$
  • These constraints encode the maintenance of specified contact modes (e.g., sticking) via polyhedral approximations of friction cones.

B. Trajectory-Following (Task Constraints)

• Desired generalized velocity: $G v = b_G$, where $G \in \mathbb{R}^{m \times n}$, $b_G \in \mathbb{R}^m$.

C. Hybrid Servoing Requirement

• Select $T$, $n_{av}$, $n_{af}$, $w_{av}$, $\eta_{af}$ such that:
  1. The imposed velocity commands, consistent with contact constraints, realize the trajectory goal.
  2. The force commands, together with the constraints, ensure guard conditions are met.

D. Optimality Criterion

• The kinematic “crashing index” $\kappa([\hat J; \hat C])$ (where $\hat J$ and $\hat C$ are orthonormal row bases of the constraint and velocity subspaces) quantifies robustness: minimizing $\kappa$ avoids directions where small configuration errors yield large internal forces (Hou et al., 2020).
• Force/velocity axes are chosen to minimize over-constraining while maximizing compliance for robustness.

3. Synthesis Algorithms: Iterative and Closed-Form Methods

A. Two-Stage Optimization (Hou et al., 2019)

1. Velocity-Level Synthesis:
   • Compute dimension $$n_{av} = \operatorname{rank}([J; G]) - \operatorname{rank}(J)$$.
   • Enforce that additional velocity-control axes $C$ impose $G v = b_G$ on the nullspace of $J$.
   • Optimize the velocity axes $C$ for mutual orthogonality (good conditioning) and alignment with the nullspace of the constraints (robustness):

   $$\min_{\{ k_j \}} \sum_{i \ne j} |c_i^\top c_j| - \alpha \sum_{i} \| \operatorname{Null}(J)^\top c_i \|^2,$$

   subject to linear subspace and unit-norm constraints on $c_i$ (projected gradient descent with random restarts). - Form the transform $R_a$ so that the force- and velocity-control axes are mutually orthonormal and span the desired subspaces.

2. Force-Level Synthesis:

   • With $T$ fixed, express system equilibrium equations in terms of free variables (combining $\lambda$, $\eta_u$, $\eta_{av}$).
   • Solve for these via quadratic programming (min-norm solution) subject to static equilibrium.
   • Optimize $\eta_{af}$ via a small-scale LP to satisfy guard constraints.

B. Closed-Form Solution (Hou et al., 2020)

• Replace the iterative search of velocity axes with a closed-form, orthonormal basis computation:
  1. Compute the nullspace $U = \operatorname{Null}(J)$ (instantaneous free-motion space).
  2. Extract actuated block $\bar U = \operatorname{Row}(U S_a)$ with $S_a$ as column selector.
  3. For minimal velocity axes, construct $C = K \bar U$, with $K$ an orthonormal basis of the nullspace of $([J; G])$ projected onto actuated DOFs.
  4. Force axes are completed by orthonormal complement.
  5. Feasibility conditions (Goal-Inclusion) verified via nullspace dimension checks.

• This analytical approach enables significantly faster computation ($0.1$–$0.3$ ms vs. $2$–$6$ ms in prior search-based methods) and better kinematic conditioning (Hou et al., 2020).

4. Robustness, Conditioning, and Trade-Offs

Robust HFVC is achieved by selecting velocity-control axes that are as deeply embedded as possible within the nullspace of contact constraints, thus minimizing violation risk under model uncertainties. Mutual orthogonality ensures the velocity subspace is well-conditioned, so that disturbances map minimally between axes. By minimizing the number of force-controlled axes (to the lower bound imposed by $\operatorname{rank}([J;G])-\operatorname{rank}(J)$), the method leaves maximal degrees of freedom for force compliance, lowering susceptibility to over-constraining.

Guard conditions via polyhedral approximations to friction cones supply robustness to slip and separation by ensuring adequate margins on friction and normal forces. The kinematic conditioning metric (“crashing index”) is a global optimality guarantee for the velocity-force subspace design, penalizing directions susceptible to high internal forces under small configuration errors (Hou et al., 2020, Hou et al., 2019).

5. Computational Complexity and Practical Implementation

Both optimization-based (Hou et al., 2019) and closed-form (Hou et al., 2020) methods operate efficiently for typical manipulation DOFs ($n \leq 24$). The closed-form strategy is dominated by singular value decomposition or QR factorizations for nullspace computation, with asymptotic complexity $O(n^3)$. The force subproblem is a small convex QP and LP, tractable in real-time servo loops. Compared to iterative search (multiple nonlinear optimization restarts), closed-form analytic HFVC reduces computation time for velocity-axis selection by $7$–$40\times$, with overall control loop times suitable for $100$–$250$ Hz real robot controllers.

6. Experimental Validation and Limitations

Validation of HFVC methods spans simulated random instance suites and real-robot experiments:

• Randomized simulations: On $78\,000$ test cases (planar and 3D, $6$–$24$ DOF), the closed-form method delivers lower mean crashing index and fewer ill-conditioned solutions than search-based approaches. Maximum and average velocity-axis computation times are substantially reduced (Hou et al., 2020).
• Physical robots: Tasks include block tilting (100/100 success at 50% higher speed), tile levering-up, and shared-grasp object transport. Performance exceeds prior hybrid servoing, especially in success rate and speed (Hou et al., 2019, Hou et al., 2020).
• Failure modes: Limitations arise from force-control latency and unmodeled micro-stick/slip, which margin-based guard constraints mitigate but cannot fully eliminate. The method’s guarantees hold under the quasi-static rigid-body model with holonomic, point-to-face contact and linearized friction cones. If the goal-inclusion nullspace condition fails, no HFVC can achieve the desired trajectory—this is detected algorithmically during synthesis.

7. Assumptions and Scope

Key assumptions underlying both the iterative and closed-form HFVC approaches include:

• Strictly quasi-static mechanics (no dynamic or inertial effects).
• Perfectly rigid bodies and Coulomb friction behavior.
• Restriction to holonomic (sticking) point-to-face contacts, with friction cones approximated by polyhedra.
• Accurate knowledge of contact geometry, external forces, and system Jacobians.
• Lower-level tracking of force and velocity commands (e.g., by impedance control using force/torque sensing).

No explicit support exists for point-edge or pure moment contacts, non-holonomic (sliding) constraints beyond guard-based treatment, or dynamic environments. Contact uncertainty, deformation, or compliance, if present, are not directly handled, although robust margin selection in guard constraints provides some practical tolerance to these effects.

---

References:

• (Hou et al., 2019) Robust Execution of Contact-Rich Motion Plans by Hybrid Force-Velocity Control
• (Hou et al., 2020) An Efficient Closed-Form Method for Optimal Hybrid Force-Velocity Control