PaperlayerView exact revision on arXiv

Source-linked AI summary

Task-Space Motion-Force Nonlinear Model Predictive Control for Precise and Compliant Control under Uncertainties

Quang-Nam Nguyen, Ludovic Righetti

arXiv:2610.17069v1cs.RO

TL;DR

NMPC can be difficult to tailor across platforms and tasks, and force-feedback MPC can depend on rigid-contact assumptions. This paper proposes a generic task-space NMPC with impedance regulation and an admittance force-feedback loop. Hardware experiments demonstrate precise motion-force control and compliant locomotion, with the analysis scoped to unconstrained OCPs.

  • Problem

    Designing NMPC OCPs for specific platforms and tasks demands expertise, while rigid-contact assumptions can make optimization-based interaction control sensitive to modeling errors.

    Source: §I · p. 1

  • Method

    The framework plans with task-space end-effector costs, analyzes the resulting impedance, and uses admittance to close the force-feedback loop in NMPC.

    Source: §I · pp. 1–2

  • Results

    IA-NMPC, A-NMPC, and Admittance IK maintained highly precise contact forces; IA-NMPC drew on a balloon at 0.50N with standard deviation of 0.08−0.12N.

    Source: 3) Direct motion-force control · p. 6

  • Takeaways & Limitations

    The framework was validated on a redundant manipulator for hybrid motion-force control and on a quadruped for compliant locomotion in unknown environments.

    Source: §VII · pp. 7–8

    The impedance control law is analyzed for the unconstrained OCP; the effects of active constraints such as joint limits and collision avoidance remain for future investigation.

    Source: §VII · p. 8

Abstract

from arXiv · show

In Nonlinear Model Predictive Control (NMPC), the control law is implicitly determined by the solution of an Optimal Control Problem (OCP). Consequently, OCP design is often tailored to specific platforms and tasks, demanding extensive expert knowledge and tuning effort. To address this, we propose a unified NMPC framework that directly plans and controls the robot joints subject to task-space motion and force requirements and is capable of seamlessly generalizing across multiple robotic platforms and diverse tasks. Furthermore, optimization-based methods often treat interactions as rigid contacts, making the torque solution sensitive to the modeling errors of the robot dynamics and the environment. We study the intrinsic task-space impedance of our proposed controller, its partial contraction for differential stability and differential passivity under unmodeled interactions. As a result, our framework enables closing the force-feedback loop in the high-level planning layer to achieve precise motion-force tracking performance, addressing the long-standing challenge of force-feedback NMPC. Finally, we validate our framework on physical hardware, demonstrating hybrid motion-force control on a redundant manipulator and compliant locomotion on a quadruped, while preserving core NMPC advantages such as hard constraint satisfaction, optimal redundancy exploitation, and real-time planning under unforeseen interactions.

I. INTRODUCTION

NMPC can offer online adaptation and hard-constraint satisfaction, but complex OCPs are often used offline and platform-specific controllers can be brittle to unexpected interactions. The paper proposes a generic task-space NMPC framework, using end-effector targets and impedance regulation to support diverse robots and tasks.

I. INTRODUCTION

  • OCPs are often used for offline trajectory optimization because long horizons with complex costs and constraints are computationally burdensome, while real-robot tracking can require separate controllers.Platform- and task-specific formulations and strong contact assumptions can also make whole-body NMPC brittle to unexpected interactions.Source: §I · p. 1
  • The proposed task-space NMPC uses platform-agnostic end-effector targets and a generic OCP to generate real-time robot motion with passive task-space impedance.The framework is intended to generalize across robot platforms and diverse tasks while reducing reliance on rigid-contact assumptions.Source: §I · pp. 1–2

1) Control by optimization:

  • The framework replaces separate inverse kinematics, inverse dynamics, and PD control with receding-horizon optimal control based on task-space goals.The paper frames this as an alternative to decoupling task-space planning from joint-space planning and control.Source: 1) Control by optimization · p. 2

2) Impedance and Admittance:

  • Unlike closed-form methods that specify spring- and damper-like constants, the OCP can scale impedance stiffness through its weight matrices.The paper presents impedance and admittance as complementary approaches to end-effector contact behavior.Source: 2) Impedance and Admittance · p. 2

3) Force-feedback MPC:

The framework handles force-feedback tasks without imposing rigid contact constraints on interactions, using task-space planning and an admittance loop around a generic OCP. Its predictive dynamics enforce rigid contacts only where specified for standing feet.

3) Force-feedback MPC:

  • Rigid-contact force-feedback MPC can over-constrain the robot and depend on accurate contact timing and location, whereas this method reasons about interactions through task-space impedance without rigid end-effector constraints.The authors attribute this to a mismatch between the predictive model and actual interaction dynamics.Source: 3) Force-feedback MPC · p. 2

II. METHODOLOGY OVERVIEW

  • Task-space plans, including discrete contact placements or reaching targets and continuous paths, feed a generic OCP that computes optimal joint and torque trajectories.The high-level planner can be a planning- or learning-based method that outputs end-effector plans; the formulation avoids nominal joint-trajectory tracking.Source: §II · pp. 2–3
  • The task-space cost formulation allows impedance stiffness regulation and closes the force-feedback loop through admittance, forming IA-NMPC.The OCP can produce joint position, velocity, and torque trajectories for low-level control.Source: §II · p. 3

A. Clarifications on dynamics model with contacts

  • The predictive dynamics omit external interaction wrenches and impose rigid-contact constraints only on standing feet of floating-base robots.The paper argues that unplanned or non-rigid contacts are difficult to predict, and that model mismatch with the cost function characterizes a regulatable task-space impedance.Source: A. Clarifications on dynamics model… · p. 3

B. Optimal Control Problem

The section recovers a Riccati recursion from the OCP’s KKT conditions to characterize its optimal trajectories, assuming solver convergence. The analysis is explicitly focused on the unconstrained OCP.

B. Optimal Control Problem

  • The control-law analysis focuses on the unconstrained OCP.This scope distinguishes the derivation from active inequality constraints.Source: B. Optimal Control Problem · p. 3
  • The optimal state and control trajectories are required to satisfy the KKT conditions, with dynamics linearized at the current guess.The passage defines the state as configuration and velocity and gives derivatives of the cost and dynamics in the KKT notation.Source: B. Optimal Control Problem · p. 3
  • Assuming convergence after sufficient solver iterations, the KKT conditions yield a backward Riccati recursion initialized by the terminal cost gradient.The stated convergence assumption is that changes in state and control trajectories approach zero.Source: B. Optimal Control Problem · p. 3

IV. TASK-SPACE IMPEDANCE NMPC

The section uses end-effector pose, velocity, and control regularization costs rather than nominal joint-position tracking. It combines forward kinematics and dynamics under a gravity-compensation reference-control assumption.

IV. TASK-SPACE IMPEDANCE NMPC

  • The generic OCP’s optimal control law is analyzed through Riccati recursion to show how it characterizes task-space impedance.The authors use this insight to regulate task-space impedance and close the NMPC force-feedback loop in the following section.Source: §IV · pp. 3–4

A. Cost function

  • The cost uses end-effector pose, velocity, and control regularization, omitting joint-position cost while retaining joint-velocity cost for smooth trajectories.The end-effector pose cost is used to plan trajectories toward a goal.Source: A. Cost function · p. 4

B. Combined Forward Kinematics-Dynamics

  • The combined forward kinematics-dynamics formulation assumes a reference control typically set to gravity compensation when a robot end is in rigid ground contact.The passage notes that modeling errors such as inertial-matrix and joint-friction errors can be included in the external force.Source: B. Combined Forward Kinematics-Dynamics · p. 4

C. Recovering the control law

The cost and dynamics are processed backward through a Riccati recursion; MPC then applies the first control from the resulting plan.

C. Recovering the control law

D. Task-space Impedance of our OCP formulation

The OCP’s end-effector costs yield a task-space impedance law, and its stiffness and damping can be scaled through cost weights.

D. Task-space Impedance of our OCP formulation

  • The closed-loop task-space dynamics obey an impedance law with inertia Λ, damping-related terms, and stiffness term J†SᵀKP∆p.The dynamics are obtained by substituting the gravity-compensated control wrench into the task-space dynamics.Source: D. Task-space Impedance of our OCP… · p. 4
  • The impedance stiffness KP and damping KD can be scaled through cost weight matrices QP and QV as they backpropagate through the Riccati recursion.Source: D. Task-space Impedance of our OCP… · p. 5
  • The task-space impedance law attracts the end-effector to a steady-state equilibrium and can prevent deviation under unmodeled forces Fe.Source: D. Task-space Impedance of our OCP… · p. 5

V. MOTION-FORCE NMPC

The analysis constructs virtual variation dynamics for the closed-loop system and defines a differential Lyapunov, also differential storage, function.

V. MOTION-FORCE NMPC

  • The partial-contraction analysis uses a virtual state z = qdot, external forces Fe, and relative end-effector goal twist veg to form variation dynamics.Source: A. Partial Contraction and… · p. 5
  • The analysis defines a differential Lyapunov function that also serves as a differential storage function.Source: A. Partial Contraction and… · p. 5
  • The text defines a differential Lyapunov function, also described as a differential storage function, and notes δzᵀ(Ṁ − 2C)δz = 0 together with δṗe = Jeδz.Source: A. Partial Contraction and… · p. 5

1) Partial contraction:

The controller’s damping condition supports partial contraction and differential passivity; an admittance layer adjusts the goal pose for motion and force tracking.

1) Partial contraction:

  • If SᵀKD ≥ αI for α > 0, the system is partially contracting and converges to the task-space equilibrium while retaining joint-space freedom.The retained freedom supports optimal redundancy exploitation in manipulators and adapting behaviors of legged robots.Source: 1) Partial contraction · p. 5

2) Differential passivity:

  • With moving goals and external-force variations, differential passivity holds if SᵀKD ≥ 0, and strict differential passivity holds if SᵀKD > 0.The storage rate is bounded by energy transfer from external forces and the virtual energy created by the moving goal.Source: 2) Differential passivity · p. 5
  • The condition for both partial contraction and differential passivity is SᵀKD ≥ αI, α > 0, achievable by tuning the cost weight matrix Q.Source: 2) Differential passivity · p. 5

B. Impedance-Admittance NMPC

  • Admittance NMPC adjusts the relative end-effector pose by updating the goal-frame transformation using an integrated desired dynamics.The desired dynamics use positive-diagonal matrices for desired inertia, damping, and stiffness.Source: B. Impedance-Admittance NMPC · p. 5

1) Staying mode:

  • In staying mode, the desired pose is fixed and the desired velocity, acceleration, and force are zero, returning the object to its desired equilibrium.Source: 1) Staying mode · p. 5

3) Force tracking:

  • Setting the desired pose to the goal with zero desired velocity, acceleration, and force allows the object to move to any pose in space.Source: 3) Force tracking · p. 5
  • In force tracking, setting the desired pose to the goal and desired force nonzero yields proportional force control, with an optional derivative term for PD force control.Source: 3) Force tracking · p. 5
  • The OCP’s optimal joint and torque trajectories support position-controlled A-NMPC and torque-controlled IA-NMPC.Source: 3) Force tracking · p. 5

VI. EXPERIMENTS

The experiments examine task-space impedance and force tracking on a 7-DOF manipulator, including drawing across flat and deformable surfaces. They also compare controller setups and illustrate real-time constraint addition under disturbances.

A. Experiment 1: Task-space Impedance in manipulation

  • The pressing test found that increasing the end-effector cost’s z weight increased contact stiffness, while the force-position relationship was state-dependent.The force sensor measured vertical contact force as the goal pose moved below the surface; the results support adapting goal pose for force and cost weights for stiffness.Source: A. Experiment 1: Task-space Impedance… · p. 6

B. Experiment 2: Drawing on flat and deformable surfaces

  • Drawing tasks set target forces of 1.0N on a board, 2.0N on paper, and 0.5N on a balloon.Massage/polishing tasks can use a desired contact force such as 10N; admittance coefficients were tuned for each material and method.Source: B. Experiment 2: Drawing on flat and… · p. 6
  • The drawing comparison uses IA-NMPC in pure torque mode, A-NMPC in position mode, and three baseline controllers, with a 100Hz update and 20-node horizon.Experiments use raw force/torque sensor data without an additional estimator or filter.Source: B. Experiment 2: Drawing on flat and… · p. 6

1) Classical admittance control (Admittance IK):

  • Admittance IK passes admittance-adjusted desired end-effector velocity to a QP solver to compute joint velocities.The experiments also test FDDP and mim solvers; both produce compliant behavior, and mim solves the inequality requirements as hard constraints.Source: §VI · p. 5, 1) Classical admittance control… · p. 6

VI. EXPERIMENTS

  • Fig. 6 depicts drawing under disturbances with real-time constraint addition.The figure caption identifies the scenario; the passage does not specify the visual encoding or outcome.Source: Figure 6 · p. 6

2) Torque-control admittance (Admittance ID):

The experiments compare torque-control admittance and direct motion-force control, then demonstrate object-property estimation for SE(3) manipulation. The reported results include force-tracking performance, motion-tracking trade-offs, and a mass and center-of-mass estimate.

3) Direct motion-force control:

  • IA-NMPC, A-NMPC, and Admittance IK maintained precise contact forces; IA-NMPC drew on a balloon at 0.50 with 0.08−0.12N standard deviation without knowing its curvature or deformation.IA-NMPC had slightly lower position accuracy because it complied with friction in other directions, with position standard deviations of 0.65−1.48mm; stiffness can be adjusted when greater accuracy is needed.Source: 3) Direct motion-force control · p. 6, Table I · p. 7

2) Torque-control admittance (Admittance ID):

  • Torque-control Admittance ID tracked force well but could not plan after friction caused reference deviation, resulting in poor massage motion tracking.Direct force control performed poorly in drawing when relying on a flat-surface model, and improved in massage where rigid contact was more relevant under high contact forces.Source: 3) Direct motion-force control · p. 6

3) Direct motion-force control:

  • At the 10N massage force, IA-NMPC’s advantage over Admittance IK was more significant as stiffer contact dynamics challenged Admittance IK.IA-NMPC’s impedance layer acts like springs in series, softening contact dynamics; its MPC framework can add and solve hard constraints in real time during human interaction.Source: 3) Direct motion-force control · pp. 6–7

C. Experiment 3: SE(3) admittance with a heavy object

  • The demonstrated SE(3) pipeline manipulates unknown heavy objects without including an object model in the OCP.The authors identify potential usefulness for human-robot collaboration tasks.Source: C. Experiment 3: SE(3) admittance with… · p. 7

1) Estimation of object’s inertia properties:

  • Without an object model, the method estimates mass by averaging and center of mass by least squares using force/torque data collected at different orientations.For the 3.48kg plate and 0.42kg coupling, it estimated total mass as 3.93kg and center of mass at [0.00, 0.00, 0.04]m in the end-effector frame.Source: 1) Estimation of object’s inertia… · p. 7

2) Estimation of external interaction’s wrench:

The section reports wrench estimation and task-space impedance demonstrations spanning object manipulation and quadruped interactions. It also identifies the scope of the impedance analysis and a computational limitation.

2) Estimation of external interaction’s wrench:

3) Object manipulation in SE(3):

  • SE(3) manipulation works in staying and floating modes while object inertia is omitted from the robot model; the framework also supports real-time predictive planning under whole-body interactions.The implementation uses the SE(3) admittance in equation (26).Source: 3) Object manipulation in SE(3) · p. 7

D. Experiment 4: Compliant locomotion for a quadruped in an unknown, unsteady environment with limited sensing

  • With only joint and gyroscope feedback, Go2 compliantly adapts to unexpectedly moving obstacles and computes recovery trajectories despite absent contact detection and odometry.The experiment removes contact detection and translational position/velocity odometry feedback.Source: D. Experiment 4: Compliant locomotion… · p. 7

E. Experiment 5: Quadruped Go2 carries human on a chair

  • A few COM-cost and target changes let Go2 pull a chair carrying a person, demonstrating behavior customization without reformulation or retraining.The reported behavior uses passive impedance without force feedback, unlike the cited teacher–student approach [31], which used privileged external-force information during teacher training.Source: E. Experiment 5: Quadruped Go2 carries… · p. 7

VII. CONCLUSIONS

  • The framework regulates contact forces by adapting end-effector cost weights and goal poses, and is validated on compliant quadruped locomotion and constrained manipulator motion-force tasks.The conclusion also reports differential stability and passivity guarantees.Source: §VII · pp. 7–8
  • The impedance law is analyzed for unconstrained OCPs, while computation time grows with degrees of freedom and horizon, introducing delay and limiting control frequency.Source: §VII · p. 8
Loading 2610.17069v1…