Latest / Embodied AI 101 / Two MPCs, One Robot: Bézier Curves for Fast, Compliant Mobile Manipulation
Transcript
- Two MPCs, One Robot: Bézier Curves for Fast, Compliant Mobile Manipulation. Efficient Whole-Body Model Predictive Control for Online Compliant Dual-Arm Mobile Manipulation • Wenqian Du, Ran Long, João Moura, Jiayi Wang, Saeid Samadi, and Sethu Vijayakumar • University of Edinburgh and The Alan Turing Institute • Journal of Field Robotics • 2026. At first glance, this is a paper about dual-arm mobile manipulation. More precisely, it is a paper about trajectory representation: how a seemingly modest change in optimization variables can turn an unwieldy whole-body controller into something fast enough to run online. The target behavior is ambitious. A mobile robot with two arms should pick up and transport a large object, maneuver its base and upper body together, avoid obstacles that may move, maintain safe contact forces, yield when physically disturbed, and respect joint, velocity, and collision constraints. The robot is not given a dense reference trajectory. It is given an object-level goal, and the controller must continually generate the intervening motion. Du and colleagues address this with two coupled model predictive controllers. A long-horizon task-space MPC plans the motion of the two end effectors. A shorter-horizon whole-body MPC converts that plan into feasible base and joint motion while incorporating predictive admittance control. Both optimization problems represent trajectories using a small number of Bézier control points rather than treating position, velocity, and acceleration at every time knot as separate decision variables. An open preprint posted on October 30, 2024 carried the longer title An Efficient Representation of Whole-body Model Predictive Control for Online Compliant Dual-arm Mobile Manipulation; that preprint contains the technical formulation and experiments discussed here. The result is not a learned policy, a new nonlinear solver, or a torque-level whole-body dynamics controller. It is a carefully constructed optimization architecture whose central claim is that choosing the right finite-dimensional curve family can matter as much as choosing the solver. The Real Problem Is Coupling Everything Online. Dual-arm mobile manipulation combines several difficulties that are individually manageable but collectively expensive. First, the two end effectors are not independent. When carrying a box between two rigid palms, their translations and orientations should evolve coherently. A trajectory that is collision-free for one hand may be infeasible for the other arm or may rotate the object into an obstacle. Second, the base and arms should move simultaneously. A conventional pipeline might navigate the base to a stance, stop, and then manipulate. That simplifies planning but sacrifices reachability, smoothness, and execution time. Whole-body optimization can instead let the base translate or turn while the arms reposition, using redundancy to navigate around obstacles and remain away from joint limits. Third, manipulation introduces force control. A purely geometric trajectory may demand the wrong separation between the palms, collide with an imperfectly modeled object, or generate large forces when the environment pushes back. Compliance must modify the desired motion without violating the rest of the robot’s constraints. Finally, these decisions occur on different timescales. Smooth obstacle avoidance benefits from looking many seconds ahead, especially for a large mobile platform. Force adaptation and whole-body feasibility need a shorter horizon that can be re-solved quickly. Trying to put both concerns into one long-horizon, high-dimensional nonlinear program creates a large search space and leaves the optimizer vulnerable to poor local minima. The hardware makes the transcription issue especially visible. The authors’ EVA platform has 18 optimized degrees of freedom: planar base position and yaw, a chest joint, two head joints, and 12 arm joints. Its mecanum-wheel base accepts velocity commands, while its upper body accepts position commands. Wrist force sensors are available at both rigid end effectors. The optimizer must therefore produce a base velocity trajectory and an upper-body position trajectory that are mutually consistent in time. In a standard direct transcription, joint position and joint velocity might both appear as decision variables at a sequence of knots, with equality constraints approximating their relationship through Euler integration or another numerical scheme. With coarse knots, this relationship is inaccurate. With dense knots, the program becomes large. For EVA, inconsistency is not merely an internal numerical error: it can manifest as incompatible commands being sent to two differently controlled parts of the physical robot. The Architecture: A Global Guide and a Local Realizer. The proposed controller separates the problem into two receding-horizon nonlinear programs. The first is MPC-T, where “T” denotes task space. It receives the desired object pose and plans long-horizon position and orientation trajectories for both end effectors. This layer reasons about the mission in an object-centric space, including obstacle avoidance and approximate limits on how quickly the mobile base can move. It does not contain the full 18-degree-of-freedom kinematic model. The second is MPC-W, the whole-body controller. It receives the end-effector trajectory from MPC-T as a reference, then optimizes the robot’s complete configuration over a shorter horizon. It tracks and, importantly, corrects the task-space plan while enforcing joint limits, velocity limits, simplified collision constraints, and a force-to-motion admittance relationship. It then sends a position command to the upper body and a velocity command to the base. Both optimizers are re-solved after the robot moves. MPC-T therefore does not produce a global trajectory that is blindly followed to completion. Its output is a continually updated attractor for MPC-W. If an obstacle moves, the long-horizon reference changes. If the task-space reference is not physically reachable, MPC-W modifies it locally. If measured contact force deviates from the target, MPC-W introduces a compliant displacement. The authors call this a bilevel MPC framework. It is useful to interpret “bilevel” architecturally rather than as a single nested mathematical program whose lower-level optimality conditions are embedded in an upper-level problem. These are two coupled MPCs connected through a reference trajectory and the measured robot state. That separation is precisely what makes the computation tractable. It also establishes a productive asymmetry. MPC-T is optimistic: it sees a compact task-space problem and can plan far ahead. MPC-W is conservative: it sees the detailed robot and is allowed to reject or reshape part of the optimistic plan. The first layer guides the optimizer away from bad local solutions, while the second layer protects feasibility. The paper’s experiments show that giving MPC-W an online, collision-aware reference reduces both solve time and awkward last-moment maneuvers compared with tracking a fixed, inappropriate reference. Why Bézier Curves Change the Optimization. The central technical device is to represent each trajectory with a Bézier curve. A Bézier curve is determined by a finite set of control points and a normalized time variable running from zero to one. Its velocity and acceleration are themselves Bézier curves whose control points can be calculated directly from first and second differences of the original control points. The optimizer therefore needs to choose only one set of variables. Position, velocity, and acceleration remain analytically tied together. This eliminates the approximate state-transition constraints that would otherwise connect independently optimized positions and velocities at adjacent knots. In the whole-body controller, the derivative used to command the base is exactly the derivative of the configuration curve used to command the upper body—exactly within the selected polynomial trajectory family, at least. That qualification matters. Bézier transcription does not make the physical robot model exact. It removes one source of discretization inconsistency by replacing a collection of loosely connected states with a differentiable curve. In exchange, the optimizer is restricted to trajectories expressible by a Bézier polynomial of the chosen degree. The method trades integration error for basis approximation error. The representation separates two quantities often conflated in trajectory optimization:. Control points determine the number of decision variables and the expressive capacity of the trajectory. Evaluation knots determine where nonlinear costs and constraints are checked. The authors can therefore evaluate forward kinematics or force-tracking costs at more knots without introducing a new position and velocity variable at every one. More evaluation knots still mean more function evaluations and constraints, but the growth is much slower than in a conventional transcription. Bézier curves also have a convex-hull property. Every point on a Euclidean Bézier curve lies inside the convex hull of its control points. If every joint-position control point lies inside a box defined by the joint limits, the complete joint trajectory respects those bounds. The same reasoning applies to velocity and acceleration curves because their control points are known differences of the position control points. Terminal rest conditions are particularly simple: arranging the final two or three control points appropriately produces zero terminal velocity and acceleration. This property is strongest for convex bounds such as joint intervals, componentwise velocity limits, or containment within a convex corridor. It does not automatically certify every collision constraint. The free space outside a spherical obstacle is nonconvex, so merely placing each control point outside the obstacle does not prove that the curve between them stays outside. The paper evaluates end-effector obstacle distances at selected knots and constrains base control points using distance inequalities. Those are hard inequalities in the nonlinear program, but they should not be read as a general continuous-time collision certificate. Keeping Orientation on the Unit-Quaternion Manifold. The orientation trajectory is a more unusual part of the method. Directly fitting Bézier curves to the four components of a quaternion requires unit-norm equality constraints. Enforcing those constraints only at evaluation knots does not guarantee that the curve remains normalized between them, while enforcing them densely makes the nonlinear program expensive. Instead, the authors optimize three parameters per end effector: a rotation angle and two spherical coordinates describing the rotation axis. A standard axis-angle mapping converts those parameters to a quaternion. Because the mapping constructs the quaternion through sine and cosine terms, every resulting orientation is normalized without an explicit unit-quaternion constraint. For two hands, this reduces the orientation representation from eight scalar quaternion components to six parameters. This is computationally effective, but it is not a globally regular coordinate system for rotation. At zero rotation, the axis is arbitrary; the spherical description of the axis has pole singularities; and equivalent axis-angle or antipodal quaternion representations can create wrapping choices. Penalizing derivatives of these coordinates is also not identical to penalizing physical angular velocity or geodesic distance on the rotation group. The contribution should therefore be understood narrowly but positively: the mapping guarantees unit quaternions and removes costly equality constraints in the tested motion regime. It does not solve the global topology of optimization on the rotation group. MPC-T: Long-Horizon Planning for Two Collaborating Hands. MPC-T jointly plans six translational coordinates—three for each end effector—and six orientation parameters. The paper uses eight control points, allowing relatively complicated smooth paths while keeping the nonlinear program compact. One part of the objective encourages the palms to face one another. Rather than explicitly enforcing a rigid object model, the controller penalizes components of the vector between the palm centers along each palm’s local horizontal axes. Informally, it tries to align the line joining the hands with the normal direction of both palms. Other terms penalize translational and rotational curve velocity and acceleration, producing short, smooth motion. The constraints anchor the first control point at the measured pose and the last at the goal. The final control points are arranged to bring both translational and rotational motion to rest. Obstacle avoidance is expressed through the midpoint between the hands, and bounds on translational velocity and acceleration approximate the capabilities of the mobile base. The task-space horizon decreases as the robot approaches the current goal. This is deliberately an approximation. MPC-T does not check arm reachability, joint limits, self-collision, or the detailed geometry of the base and carried object. It does not ensure that the separation between the palms matches the object width. That is why MPC-W is permitted to correct its output. Likewise, the equations do not introduce an explicit model of obstacle velocity or uncertainty. My reading is that the demonstrated dynamic-obstacle behavior comes primarily from fast closed-loop replanning as updated obstacle positions arrive, rather than from forecasting obstacle trajectories over the full horizon. That distinction matters in settings with latency or rapidly moving humans. MPC-W: Whole-Body Realization with Predictive Admittance. MPC-W parameterizes the complete 18-dimensional robot configuration with another Bézier curve, typically using six control points. At each evaluation knot, forward kinematics maps that configuration to both end-effector poses. Its first objective tracks the position and quaternion references supplied by MPC-T. A second objective penalizes upper-body movement, encouraging the controller to use the mobile base when appropriate and preserving arm workspace for future manipulation. Further regularization penalizes velocity and acceleration of both the joint trajectory and the compliant motion-response trajectory. Joint-position and velocity limits are imposed on the corresponding control points, allowing the convex-hull property to extend those bounds over the entire curve. Collision constraints separately represent the center of the two hands and the mobile base, rather than surrounding the complete robot with one large safety sphere. This simplification lets EVA turn through a narrow passage, but it leaves the arm links, individual hands, carried object, and self-collision only indirectly represented. What “Predictive Admittance” Means Here. The force-control component introduces a translational offset between the MPC-T reference and the motion MPC-W actually tracks. That offset is another Bézier curve. At each knot, a virtual spring-damper relationship connects the measured interaction force, the offset and its velocity, and an optimized force. The objective pushes that optimized force toward a desired force profile expressed in the local frames of the hands. If the measured normal force becomes too large, the optimizer can move a hand away from the nominal reference; if contact is too weak, it can move inward. Because the complete response curve is optimized alongside the whole-body trajectory, the adjustment can account for upcoming joint and collision constraints. “Predictive” does not mean that the controller learns or simulates detailed future contact dynamics. As written, it propagates the current force measurement through a virtual compliance model while optimizing a future motion response. The authors omit the virtual inertia term, arguing that the base acceleration is limited to one meter per second squared and each end effector has a mass of 0.33 kilograms. The resulting controller is essentially predictive spring-damper admittance embedded inside a constrained trajectory optimizer. That is still a meaningful distinction from instantaneous admittance control. An instantaneous controller might command a displacement that causes a joint-limit violation or steers the base toward an obstacle. Here, the compliant response and whole-body configuration are chosen together. The final command is sampled 20 milliseconds into the optimized trajectory. The upper body receives the corresponding configuration, while the base receives the analytic derivative of the same curve, transformed into the base frame. This is the practical payoff of the representation: the heterogeneous command interfaces share one internally consistent trajectory. What the Comparisons Actually Show. The first simulation isolates task-space planning. All variants use eight evaluation knots. A conventional discretization optimizes position, velocity, quaternion, and quaternion-rate states, for 224 decision variables. Representing position and raw quaternion components with Bézier curves reduces that to 112 variables. The proposed axis-angle-based representation needs 96. Average MPC-T solve time falls from 60.8 milliseconds for the conventional transcription to 19.4 milliseconds for raw quaternion Bézier curves, and then to approximately 6.2 milliseconds for the proposed orientation representation. The corresponding standard deviations are 36.8, 8.9, and 2.4 milliseconds. This is a large gain, although part of it comes from removing quaternion norm constraints rather than from Bézier curves alone. The whole-body comparison is even more informative. With a five-second horizon and six knots, discretized MPC-W uses 216 variables, runs in 12 milliseconds, and produces a mean absolute tracking error of 21.6 percent. Increasing the discretization to 26 knots improves error to 6.3 percent, but expands the program to 936 variables and increases average solve time to 60.1 milliseconds. The Bézier controller uses 108 trajectory variables in both cases because it retains six control points. With six evaluation knots, it runs in 9 milliseconds with 7.7 percent error. With 26 evaluation knots, it improves to 5.1 percent error while taking 12.5 milliseconds. More evaluation points improve the fidelity of the nonlinear tracking objective without multiplying the trajectory-state variables. As additional capabilities are enabled, the gap widens. For motion tracking alone, the reported averages are 5.1 milliseconds for the Bézier formulation and 8.1 milliseconds for discretization. Adding obstacle avoidance raises the proposed method to 7.1 milliseconds, while the discretized formulation grows by roughly a factor of three. With force control included, the proposed MPC-W uses about one-third of the baseline’s computation time and has substantially lower timing variance. The real-robot demonstrations cover four useful failure modes:. During dynamic obstacle avoidance and push recovery, MPC-T looks as far as 14 seconds ahead while MPC-W uses a two-second horizon. Their average solve times are 5.4 and 13.3 milliseconds. In the narrow-passage task, MPC-W extends to three seconds and turns the base before entering a corridor that is too narrow for sideways travel. The two average solve times are 5.5 and 13.2 milliseconds. In coordinated upper- and lower-body manipulation, EVA raises the box over one obstacle and twists its base around another. MPC-T and MPC-W average 5.5 and 14.6 milliseconds. When turning a wheel whose curved geometry is unknown, MPC-T initially proposes nearly straight hand paths. Admittance in MPC-W bends the executed motion around the wheel. Peak normal force falls from 144 newtons without admittance to 40 newtons with it, while the two optimizers average 5.2 and 11.6 milliseconds. These are compelling demonstrations of interaction between the layers. MPC-T can be geometrically wrong about the wheel yet still provide useful global progress. MPC-W can correct that plan through measured force without abandoning the task. The narrow-passage experiment shows the reverse relationship: local whole-body constraints reshape a task-space plan that did not account for the base footprint. How This Relates to Prior Mobile-Manipulation MPC. Two earlier lines of work are especially relevant. Stouraitis and colleagues’ Online Hybrid Motion Planning for Dyadic Collaborative Manipulation via Bilevel Optimization also used a two-level structure, but for a different source of complexity. Its outer level searched over discrete contact choices and grasp sequences, while its inner trajectory optimization produced continuous motions and forces for human-robot co-manipulation. Du and colleagues retain the idea that one level can provide structured guidance to another, but they focus on two continuous receding-horizon programs, fixed hand contacts, whole-body mobility, and compliant execution. Spahn, Brito, and Alonso-Mora’s Coupled Mobile Manipulation via Trajectory Optimization with Free Space Decomposition took a more monolithic approach. It optimized the base and arm together in one receding-horizon problem and represented free space around major body segments using convex polyhedra, keeping the number of collision inequalities independent of raw obstacle count. That work reported a 48 percent reduction in execution time from coupled base-arm planning. The present paper handles a higher-dimensional dual-arm platform and adds task-space guidance and force adaptation, but uses considerably simpler collision proxies. The Bézier component also connects to spline-based aerial-robot planning, where convex-hull properties are commonly used to generate smooth trajectories through static and dynamic environments. The novelty is not the existence of Bézier trajectory optimization by itself. It is the consistent use of that representation across dual-end-effector translation, rotation, whole-body configuration, and compliant motion response. What Is Genuinely Strong. The paper’s strongest contribution is conceptual economy. One representation addresses several apparently separate problems:. It reduces decision-variable count. It makes position and velocity analytically consistent. It gives a convenient mechanism for terminal smoothness. It permits continuous enforcement of convex box limits. It removes unit-quaternion equalities through a lower-dimensional orientation mapping. And it allows compliant displacement to be optimized with the same machinery as joint motion. The hierarchy is also well matched to the physical task. Large-object transportation naturally has a slow object-level intention and a fast feasibility-and-contact layer. MPC-T does not need to understand every joint to steer the system around a distant obstacle. MPC-W does not need to rediscover the entire route while reacting to a force disturbance. Finally, the experiments are better than a single scripted pick-and-place. They probe moving obstacles, external pushes, narrow geometry, coordinated redundancy, and unknown object shape. In each case the interesting behavior arises from communication between the two optimizers, not merely from replaying a precomputed path. Where the Claims Need Qualification. The principal comparison is against a straightforward discretized nonlinear MPC. It is a useful controlled baseline, but not the strongest possible alternative. Modern direct collocation, multiple shooting, real-time iteration SQP, sparse condensing, differential dynamic programming, or custom code generation could reduce both integration error and solve time. The results demonstrate that this Bézier transcription is much better than the authors’ knot-state transcription in their stack; they do not establish that it dominates every optimized MPC implementation. Likewise, “whole-body” here means whole-body kinematic motion generation, not full rigid-body dynamics. There are no joint torques, actuator dynamics, base-wheel forces, object dynamics, momentum constraints, or payload-induced balance equations. That is appropriate for a relatively slow, flat-ground, position-and-velocity-controlled platform, but the method cannot be transferred unchanged to dynamic loco-manipulation or high-speed heavy-payload motion. The paper repeatedly emphasizes hard constraints. Joint and velocity bounds genuinely benefit from the Bézier convex hull. Collision safety is less comprehensive. The model uses the hand midpoint and a base proxy rather than complete link and object geometry, and some distances are checked only at selected knots. “Hard” therefore means that these simplified inequalities are not converted to soft penalties. It does not mean certified whole-robot collision avoidance under perception and tracking uncertainty. The orientation representation warrants similar care. Unit norm is guaranteed, but smooth global optimization on the rotation group is not. A production implementation should handle angle wrapping, quaternion sign continuity, singular axis descriptions, and the antipodal ambiguity of quaternion tracking explicitly. Predictive admittance is also based on a deliberately simple contact model. It does not estimate object stiffness, friction, slip, future disturbances, or the coupled inertia of the robot-object system. Ignoring inertia may be reasonable in the reported quasi-static regime, but the 0.33-kilogram end-effector mass is not necessarily representative of the effective operational-space inertia while carrying a large object. Finally, the timing evidence is average-case rather than hard real-time. The control period is 20 milliseconds, and the separately reported MPC-T and MPC-W averages sum to roughly 17–20 milliseconds across the experiments. Standard deviations can be several milliseconds. The paper does not give end-to-end deadline traces, maximum latency, failure rates, scheduling details, or the number of successful trials. The controller is convincingly online, but deterministic 50-hertz execution is not established. How I Would Adapt This in a Modern Embodied-AI Stack. This framework is a natural execution layer beneath a learned task policy. A vision-language or vision-language-action model could select the object, manipulation phase, grasp region, and desired object pose. MPC-T could convert those sparse goals into a metric, collision-aware hand reference, while MPC-W remains responsible for robot-specific feasibility and force-limited execution. For an implementation beyond the EVA platform, I would preserve the two-timescale architecture but make several changes. I would use convex safe corridors or separating half-spaces where continuous collision guarantees are required, and model the base, arm links, hands, and carried object with capsules or convex primitives. I would replace the single global orientation chart with a Lie-group spline or a locally re-centered tangent-space representation. I would also consider piecewise Bézier or B-spline segments for longer, topologically complicated routes, because a single global polynomial has limited local support. On the interaction side, the compliant displacement could be augmented with online estimates of contact stiffness and object motion, or with a learned residual model bounded by a conventional safety layer. For harder contacts, retaining the inertial component and using a full operational-space model would be important. The authors implement their controller with OpTaS and CasADi, solve the nonlinear programs with KNITRO, and evaluate in ROS-Gazebo before moving to the real robot. That is a credible research stack, but deployment would require careful warm-starting, solver-tolerance studies, infeasibility recovery, and measurement of complete worst-case latency rather than individual average solve times. Final Take. This paper’s lesson is broader than dual-arm manipulation: before simplifying the robot model or relaxing safety constraints, simplify the trajectory representation. By expressing task motion, joint motion, and compliant response through a small set of analytically connected control points, Du and colleagues make two nonlinear MPCs practical on an 18-degree-of-freedom mobile manipulator. The method is not dynamically complete or safety-certified, and its comparisons leave room for stronger optimized baselines. But as a design pattern for quasi-static, contact-rich mobile manipulation, it is compelling: let a long-horizon optimizer say where the task should go, let a short-horizon optimizer decide how the robot can safely realize it, and ensure that both speak the same smooth curve language.