At a glance
Contact Trust Region augments a local linear contact-dynamics model with constraints on predicted contact forces and motion. Unlike a symmetric ellipsoidal bound alone, it can reject directions that require unilateral contact to pull. A relaxed version supports local trajectory optimization and feedback control; a roadmap connects useful local plans for broader reorientation. Experiments cover planar bimanual manipulation and a three-dimensional dexterous hand. The reported efficiency depends on supplied geometry, models, and task structure, rather than learning a general manipulation policy.
1 Introduction
The central question is where a differentiable contact simulator’s local approximation remains physically meaningful. Sensitivity analysis already enforces linearized force balance and perturbed complementarity, but a Taylor model alone can omit primal and dual feasibility. Contact Trust Region restores those convex constraints, connecting differentiable simulation with classical contact-force reasoning. The paper builds a local conic trajectory optimizer and a roadmap from these models, then evaluates planning, stabilization, and longer traversals separately. Its low computational cost is a result for the modeled tasks and chosen comparisons, not a universal dominance claim over reinforcement learning.
| Symbol | Shape | Meaning |
|---|---|---|
| number of positions in a system | ||
| number of actuated positions | ||
| number of object positions | ||
| system configuration vector, | ||
| actuated (robot) configuration vector | ||
| object configuration vector | ||
| object/robot configuration at the next step | ||
| control input | ||
| contact force at contact , | ||
| normal contact force at contact | ||
| tangential contact force at contact | ||
| signed distance at contact , (5a) | ||
| non-contact torque acting on object/robot | ||
| step size in seconds | ||
| robot stiffness matrix | ||
| object mass matrix | ||
| contact Jacobian at contact , (2) | ||
| feasible velocity cone at contact , (4a) | ||
| friction cone at contact , (4b) | ||
| friction coefficient at contact | ||
| (5a) | ||
| quadratic cost in contact dynamics, (3b) | ||
| linear cost in contact dynamics, (3c) | ||
| (7c) | ||
| contact dynamics smoothing parameter | ||
| (-smoothed) contact dynamics, ((14))(8) | ||
| N.A. | nominal quantities, e.g. , see (17) | |
| N.A. | deviation from nominal quantities, e.g. | |
| N.A. | Taylor expansions, e.g. | |
| trust region radius | ||
| ETR with radius , (22) | ||
| CTR with radius and smoothing , (24) | ||
| Action-only CTR, (25) | ||
| Relaxed CTR, (26) | ||
| Relaxed Action-only CTR, (27) | ||
| (action-only) motion set, ((29))(28) | ||
| contact force set at contact , (30) | ||
| generalized friction cone at contact , (31) | ||
| wrench set, (32) | ||
| motion set projected to the space of , (33) | ||
| CtrTrajOpt planning horizon in Algorithm 1 | ||
| CtrTrajOpt iteration limit in Algorithm 1 | ||
| MPC rollout horizon in Algorithm 2 | ||
| scalar trust region radius in Section 5.2.1 | ||
| state tracking quadratic in (37) | ||
| control action quadratic cost in (37) | ||
| number of MPC re-plans in Algorithm 3 |
2 Preliminaries: Contact Dynamics as Convex Optimization
The derivation uses convex quasidynamic contact simulation: configurations evolve under a strongly damped approximation without explicit velocity state. This makes each step an optimization problem whose primal solution gives motion and whose dual quantities represent contact forces. The subsequent trust region inherits both the efficiency and the approximations of this model. See Pang et al. (2023) for the cited foundation.
2.1 Setup & Notation
Configuration coordinates split into actuated robot joints and unactuated object coordinates. An action is a desired joint configuration sent to a stiffness controller, with diagonal gains. Velocities are omitted under the quasidynamic assumption; the resulting state and input are therefore different from those of a general second-order rigid-body simulator.
2.2 Equations of Motion
Force balance relates commanded stiffness forces, external loads, contact impulses, and object displacement. Contact Jacobians map generalized coordinates into normal and tangential contact directions, while a regularization term makes the object’s quasi-static displacement response well-defined. The model uses contact pairs found by collision detection and treats each force as an average over the simulation step.
Equations 1a–3c
| (1a) | ||||
| (1b) | ||||
| (2) |
| (3a) | ||||
| (3b) | ||||
| (3c) | ||||
2.3 Contact Constraints
Contact must satisfy non-penetration, friction-cone feasibility, and complementarity between separation and force, with friction opposing motion. The convex relaxation expresses these through dual cones. It introduces a modeling artifact: tangential sliding can induce normal separation. Thus the convenient convex formulation is an approximation to physical contact, not an exact general friction law. See Anitescu (2006) for the cited foundation.
Equations 4a–5c
| (4a) | ||||
| (4b) | ||||
| (5a) | ||||
| (5b) | ||||
| (5c) | ||||
2.4 Contact Dynamics as Convex Optimization
The contact equations are the optimality conditions of a second-order cone program. Its primal solution supplies the next configuration, while dual feasibility restricts contact forces. Complementary slackness here is orthogonality of the cone vectors, rather than requiring every corresponding component to vanish separately. This optimization view makes the dynamics a parameterized solution map.
Equations 6a–8b
| (6a) | ||||
| s.t. | (6b) | |||
| (7a) | ||||
| (7b) | ||||
| (7c) | ||||
| (8a) | ||||
| (8b) | ||||
| Due to the quasidynamic assumption, velocity is omitted from the state, which only consists of the system configuration . We sometimes omit the explicit dependence of , and on when it is clear from context. | ||||
2.5 Smoothing of Contact Dynamics
Contact-mode changes produce discontinuous gradients and flat regions that hinder local optimization. Log-barrier relaxation smooths the dynamics by perturbing complementarity, yielding forces even before bodies touch. These artificial forces provide directions for optimization, while their dual interpretation still respects the friction cone. Smoothing improves numerical tractability but introduces physical bias that must be controlled.
Equations 9–14b
| (9) |
| (10) |
| (11) |
| (12) |
| (13) |
2.6 Sensitivity Analysis of Smoothed Contact Dynamics
Sensitivity analysis differentiates the stationarity and perturbed-complementarity equations, then applies the implicit-function theorem to obtain derivatives of both next configuration and contact force. Action derivatives are simpler; state derivatives also involve the changing contact Jacobian and geometric curvature. The force derivatives become essential information for the proposed trust region.
Equation 15
| (15) |
3 Local Approximation of Contact Dynamics
A local optimizer needs a tractable substitute for a dynamics constraint that is itself defined by an optimization problem. The paper studies the validity of joint local models of motion and force, then separates small-perturbation accuracy from contact feasibility.
Equations 16a–16b
| (16a) | ||||
| s.t. | (16b) | |||
3.1 Linear Model over Smoothed Primal and Dual Variables
Taylor expansions of both primal motion and dual contact force satisfy the linearized force-balance and perturbed-complementarity equalities to first order. The lemma follows by combining nominal optimality with the sensitivity equations and discarding higher-order terms. It does not establish exact nonlinear dynamics or feasibility for arbitrary perturbations; those remaining inequalities motivate the trust region.
Equations 17a–21
| (17a) | ||||
| (17b) | ||||
| (17c) | ||||
| (18a) | ||||
| (18b) | ||||
| (19a) | ||||
| (19b) | ||||
| (20) |
| (21) |
3.2 Ellipsoidal Trust Region (ETR)
An ellipsoidal bound limits how far state and input perturbations move from the nominal point. Its image through a linear model is another ellipsoid, so it is symmetric even when contact permits motion only in one direction. Large ellipsoids risk approximation error, while shrinking them to fit the feasible set can make progress unnecessarily slow.
Equations 22–23
| (22) |
| (23) |
3.3 Contact Trust Region (CTR)
The full Contact Trust Region intersects an ellipsoidal perturbation bound with linearized motion and force feasibility. For a fixed current configuration, its action-only version removes state perturbations. Convexity follows from the affine models and cone constraints; equivalence to the smoothed contact equations is local and first order. Pushing examples show why dual feasibility blocks most fictitious pulling, while smoothing can leave a small mismatch. They also show that linearized non-penetration may be overly conservative. In squeezing, stronger nominal contact can instead leave the entire ellipsoid feasible, so the added constraints do not always shrink it.
Equations 24a–25e
| (24a) | ||||
| (24b) | ||||
| (24c) | ||||
| (24d) | ||||
| (24e) | ||||
| (25a) | ||||
| (25b) | ||||
| (25c) | ||||
| (25d) | ||||
| (25e) | ||||
3.4 Relaxed Contact Trust Region (R-CTR)
The relaxed variants retain friction-cone constraints on predicted contact forces but omit primal non-penetration feasibility. This often avoids an overly small region caused by unequal smoothed sensitivities of robot and object. The motion set is the convex image of that relaxed region through the local dynamics. Examples match one-step object motion and manipulator singularities better than either a symmetric ellipsoid or the full region, but dropping primal feasibility means the relaxed set is not a general certificate of exact collision-free motion.
Equations 26a–28b
| (26a) | ||||
| (26b) | ||||
| (26c) | ||||
| (26d) | ||||
| (27a) | ||||
| (27b) | ||||
| (27c) | ||||
| (27d) | ||||
| (28a) | ||||
| (28b) | ||||
Equations 29a–29b
| (29a) | ||||
| (29b) | ||||
3.5 Mechanics Derivation of the Motion Set
A complementary derivation starts with locally predicted feasible contact forces, maps them through contact Jacobians into object wrenches, and then maps net impulses into quasistatic displacement. This connects the motion set with classical wrench sets and motion cones. Bounds come from actuator perturbations and the local model rather than arbitrary independent force limits. The equivalence lemma concerns the stated quasidynamic, linearized construction, not a global reachable set of the full physical system.
Equations 30a–33b
| (30a) | ||||
| (30b) | ||||
| (30c) | ||||
| (31a) | |||
| (32a) | ||||
| (32b) | ||||
| (33a) | ||||
| (33b) | ||||
3.6 Additional Constraints
Joint-command and steady-state torque limits can further restrict the convex local problem. They constrain the same perturbations used for planning, helping keep its proposed commands within the robot model’s operating range.
3.6.1 Joint Limits
Joint limits are imposed on the commanded robot configurations through lower and upper bounds. This limits the position input, rather than independently proving every transient physical state remains inside its limits.
Equation 34
| (34) |
3.6.2 Torque Limits
The paper imposes affine bounds on a steady-state torque estimate derived from command–position error. Its printed equation (35) uses the inverse of the stiffness matrix, although equation (1) defines stiffness multiplying that error; the original notation is retained and should be checked before implementation. The quasidynamic model also omits inertial transients, so these bounds are not a full dynamic actuator-load guarantee.
Equation 35
| (35) |
4 Local Planning and Control
Local planning minimizes a trajectory objective under contact dynamics, input bounds, and an initial condition. The proposed iterative method replaces the non-smooth problem with smoothed linear dynamics and relaxed contact trust regions. This can traverse useful contact-mode changes without explicitly enumerating every mode sequence, while retaining local optimization’s dependence on initialization.
Equations 36a–36d
| (36a) | ||||
| s.t. | (36b) | |||
| (36c) | ||||
| (36d) | ||||
4.1 Trajectory Optimization with R-CTR
Each iteration rolls out a nominal input sequence, linearizes motion and force, and solves a conic optimization for all trajectory perturbations jointly. It then updates inputs and repeats the nonlinear rollout. The procedure resembles iterative linear-quadratic regulation but replaces its recursive backward pass with a joint constrained solve. The relaxed region is used because full primal feasibility often underestimates useful motion; additional joint or steady-state torque limits can be included. See Li and Todorov (2004) for the cited foundation.
Equations 37a–37h
| (37a) | ||||
| (37b) | ||||
| (37c) | ||||
| (37d) | ||||
| (37e) | ||||
| (37f) | ||||
| (37g) | ||||
| (37h) | ||||
4.2 Initial Guess Heuristic
When the robot starts far from the object, even smoothed motion gradients may be nearly zero. An initialization heuristic reverses the artificial barrier-force field and simulates the resulting attraction until contact is established, using relatively strong smoothing. This supplies a better starting command for the local optimizer; it is an initialization device, not a physically executed attractive contact force.
Equation 38
| (38) |
4.3 Examples
Toy pushing tasks illustrate how initialization and repeated relinearization can move an optimizer from separation into a useful sticking mode. Dual feasibility blocks impossible pulling, while stronger contact moves the nominal point away from the mode boundary and improves the local slope. Smoothing can still initially cause overshoot or even movement in the wrong direction for nearby goals. The examples explain a successful mechanism, not guaranteed convergence for arbitrary contact-rich tasks.
4.4 Model Predictive Control
The trajectory optimizer becomes a receding-horizon controller by executing only its first command, observing the new configuration, and replanning. The first solve uses the contact initialization heuristic; later solves reuse the previous plan. A planar two-arm example changes contact modes while translating and rotating a cylinder. Planning horizon and total episode length are distinct, and stabilization under full second-order dynamics is evaluated separately later in the paper.
5 Local MPC Experiments
Local control is tested on planar bimanual manipulation with 6 actuated and 3 object degrees of freedom, and Allegro in-hand manipulation with 16 actuated and 6 object degrees of freedom. These experiments isolate the trust-region formulation under the same convex quasidynamic simulator used for planning; transfer to second-order dynamics is evaluated separately.
5.1 Experiment Setup
Evaluation fixes diverse initial-state/goal pairs and compares final object-pose error across controllers. Robot joint targets are not prescribed, allowing each method to choose a useful grasping motion.
5.1.1 Goal Generation
Goals are deliberately locally reachable but challenging: roughly one thousand pairs per system cover broad pose differences. This is a test of local stabilization under a particular sampling scheme, rather than arbitrary global goals or an unbiased sample of all possible manipulation problems.
5.1.2 Evaluation Metrics
Final errors are reported separately as Euclidean translation distance and angular quaternion difference. Some plots add these numerically; that combined score mixes meters and radians and should be read alongside the separate quantities in the tables.
5.1.3 Implementation Details
Experiments use the authors’ existing open-source contact simulator and MOSEK for the conic subproblem, on an M2 Max laptop with 64 GB memory. Runtime claims refer to this model, implementation, and hardware. See MOSEK documentation for the cited foundation.
5.2 Effect of Trust Region Radius 𝚺 and Rollout Horizon H
The study varies trust-region radius and the number of executed MPC rollout steps while keeping the internal planning horizon at one. This separates local step authority from repeated feedback; increasing the internal horizon did not help the sampled bimanual goals in the appendix experiment.
5.2.1 Trust Region Radius
A tiny radius makes all three formulations similar but limits progress; an excessively large radius degrades the linear approximation. Relaxed contact feasibility gives the best bimanual results, while full and relaxed contact regions behave similarly for Allegro. The proposed explanation is that unilateral pushing exposes force constraints more strongly than stable opposing-finger grasps.
5.2.2 Rollout Horizon H
More rollout steps initially reduce error, followed by saturation. For Allegro, very long rollouts sometimes increase variance and error; the authors attribute this to numerical failures in its more numerous collision pairs. Longer execution is therefore not uniformly beneficial.
5.2.3 r ⋅ H : “Effective Lookahead”
The radius–rollout product is used as an approximate measure of how far repeated local actions can progress. Each plotted point selects the best radius/horizon combination at that product. Contact-constrained methods outperform the ellipsoidal baseline for sufficiently large products, but these best-of-sweep curves are not results for a single fixed controller tuning.
5.3 Can Our Method Reach Goals?
With selected hyperparameters, the relaxed region is compared with full contact feasibility and ellipsoidal bounds over every sampled pair. The figures show distributions as well as averages, making rare large errors visible.
| System | ||||
|---|---|---|---|---|
| IiwaBimanual | 1 | 2 | 0.10 | 20 |
| AllegroHand | 1 | 3 | 0.05 | 50 |
5.3.1 Goal Tracking Performance
The relaxed region has the lowest mean and variance on both systems. Its advantage is especially clear for bimanual cases where the ellipsoidal baseline proposes motion that unilateral contact cannot produce. Allegro’s full and relaxed regions are nearly tied. The explanation for why primal feasibility is much more restrictive in one system remains a hypothesis about geometry and contact configuration.
| IiwaBimanual | AllegroHand | |||
|---|---|---|---|---|
| Trans. [mm] | Rot. [mrad] | Trans. [mm] | Rot. [mrad] | |
| R-CTR | 2.0 (11.5) | 2.1 (10.1) | 2.2 (5.7) | 9.8 (26.9) |
| CTR | 9.4 (30.9) | 8.9 (33.3) | 2.2 (5.6) | 9.9 (28.3) |
| ETR | 8.6 (23.8) | 9.6 (37.9) | 4.5 (53.8) | 21.4 (136.4) |
5.3.2 Infeasibility
Infeasible local problems are rare in the reported runs, despite repeated construction of new regions. When one occurs, the rollout stops immediately. The empirical count is reassuring for these cases but does not establish recursive feasibility or guaranteed completion.
| ETR | CTR | R-CTR | Number of Goals | |
|---|---|---|---|---|
| IiwaBimanual | 0 | 0 | 0 | 1233 |
| AllegroHand | 5 | 2 | 3 | 1000 |
5.3.3 Computation Time
The unconstrained ellipsoidal variant is fastest, full contact feasibility slowest, and Allegro costs more than bimanual control. Constraint pruning and exploiting repeated sparsity are proposed optimizations. Reported timing includes the chosen conservative set of potential contacts.
| Time unit [ms] | IiwaBimanual | AllegroHand | |
|---|---|---|---|
| Per IGH | 16.8 (0.05) | 32.0 (0.6) | |
| Per | R-CTR | 5.1 (0.4) | 99.2 (28.6) |
| CTR | 7.0 (0.2) | 205.7 (32.5) | |
| ETR | 2.4 (0.2) | 52.9 (21.8) | |
| Per | 0.9 (0.05) | 5.92 (0.38) | |
6 Stabilization under Second-Order Dynamics
The next experiments test the model gap explicitly. Convex quasidynamic simulation omits inertia and permits sliding contact with a small separation gap, called hydroplaning. The controller is therefore evaluated in a second-order simulator without that artifact and on physical robots.
6.1 Implementation Heuristics for Second-order Dynamics
Two practical modifications improve transfer: execute short planned action chunks before replanning, and repeatedly use the contact-seeking initialization heuristic. The latter compensates for planned sliding forces that may not exist when the real fingers separate. These are empirical model-gap corrections, not a replacement with exact second-order contact dynamics.
6.2 Experiment Setup
Ablations compare feedback, open-loop execution, less frequent contact projection, and the three trust regions. Drake and hardware are carefully matched in geometry, gains, friction, and inertia. Object pose comes from OptiTrack; the cube uses embedded active markers and a non-penetration inverse-kinematics correction. The method is state based and assumes this reliable external pose measurement. See Castro et al. (2023) for the cited foundation.
6.3 Results & Discussion
The evaluation separates gains from feedback, renewed contact initialization, and trust-region constraints. Table 6 reports physical-unit errors for both simulated and hardware trials, while the plots expose the different failure distributions.
| IiwaBimanual | AllegroHand | |||
| Trans. [m] | Rot. [rad] | Trans. [m] | Rot. [rad] | |
| Open-loop Sim | 0.033 | 0.115 | 0.011 | 0.443 |
| (0.152) | (0.191) | (0.007) | (0.286) | |
| No-Heuristics Sim | 0.041 | 0.066 | 0.015 | 0.358 |
| (0.156) | (0.153) | (0.017) | (0.335) | |
| Closed-loop Sim | 0.020 | 0.039 | 0.014 | 0.290 |
| with R-CTR | (0.065) | (0.079) | (0.009) | (0.197) |
| Closed-loop Sim | 0.040 | 0.076 | 0.013 | 0.349 |
| with ETR | (0.167) | (0.214) | (0.010) | (0.267) |
| Closed-loop Sim | 0.031 | 0.051 | 0.013 | 0.289 |
| with CTR | (0.122) | (0.164) | (0.010) | (0.211) |
| Open-loop HW | 0.016 | 0.056 | 0.014 | 0.323 |
| (0.019) | (0.064) | (0.007) | (0.256) | |
| Closed-loop HW | 0.013 | 0.024 | 0.018 | 0.258 |
| (0.016) | (0.038) | (0.011) | (0.215) | |
6.3.1 Closed-loop vs. Open-loop
Replanning substantially improves both systems compared with executing the entire plan without object-state feedback. This demonstrates the value of correction despite an imperfect planning model.
6.3.2 AllegroHand vs. IiwaBimanual
Allegro errors are larger because lifting the cube makes slipping costly: a slip can return it to the palm and erase progress. The tabletop supports the bimanual object, so slipping usually only delays movement. The orientation-dependent results reflect this difference in physical support.
6.3.3 Closed-loop vs. No Heuristics
Frequent contact-seeking initialization reduces extreme errors and shortens robot paths more clearly than it changes mean error. It reduces large recovery motions after losing contact and helps prevent the object falling from its support, although contact loss is not eliminated.
Equation 39
| (39) |
6.3.4 R-CTR vs. ETR/CTR
The relaxed region gives the best reported combined-error distributions, with a clearer advantage on the bimanual task. Full and relaxed contact regions are close on Allegro, and both improve on ellipsoidal constraints. Individual translation and rotation columns do not all rank the methods identically, so the combined score should not be read as dominance on every metric.
6.3.5 Hardware Experiments
Hardware evaluation uses 50 spatially distributed initial-state/goal pairs per system. Mean errors resemble the matched second-order simulation, supporting transfer under the carefully calibrated conditions. This is 100 local hardware trials across two platforms, distinct from the later long roadmap traversal.
7 Generating Goal-conditioned Contact Configurations
Global manipulation can require moving away from the goal or repositioning before making progress. The paper therefore searches for advantageous contact configurations, then links them with local control. It assumes the object can remain stable while collision-free motion repositions the fully actuated robot; that assumption is essential to separating grasp selection from transit planning.
7.1 Problem Specification
For fixed current and desired object poses, optimize the robot configuration subject to joint limits and non-penetration. Its score combines predicted finite-horizon control cost with a preference for disturbance-resistant contact geometry.
Equations 40a–40c
| (40a) | ||||
| s.t. | (40b) | |||
| (40c) | ||||
7.1.1 Finite-Horizon Value Function of the MPC Policy
A candidate grasp is scored by rolling out MPC and evaluating its accumulated objective, with no penalty on desired robot joint positions. Even a simple pushing example yields a multimodal landscape, motivating sampled global search rather than relying on a single local descent.
Equations 41a–41c
7.1.2 Robustness Regularizer
The robustness term rewards an origin-centered inscribed sphere in an approximate wrench set, representing resistance to disturbances from arbitrary directions. The set is estimated by sampling the relaxed action region, rejecting infeasible forces, and taking a convex hull. This is a sampled, local model-based metric; it is not a hardware disturbance guarantee. See Ferrari and Canny (1992) for the cited foundation.
Equations 42a–43b
| (42a) | ||||
| s.t. | (42b) | |||
| (42c) | ||||
| (43a) | ||||
| s.t. | (43b) | |||
7.1.3 Total Cost
The total objective subtracts a weighted squared robustness radius from the MPC cost. The weight trades short-term goal progress against contact configurations that can oppose a broader set of disturbances.
Equation 44
| (44) |
7.2 Solving ( 40 ) by Sampling-based Optimization
Rejection sampling and best-candidate selection handle the nonconvex grasp objective without differentiating through the whole rollout. Direct sampling is inefficient in high-dimensional joint space, so Allegro uses a reduced fingertip model and projection heuristics described in the appendix.
| Task | Position [mm] | Rotation [mrad] | Time [s] |
|---|---|---|---|
| Yaw | 12.05 (3.49) | 32.41 (27.14) | 57.83 (2.08) |
| Yaw | 9.85 (10.88) | 15.72 (21.08) | 57.28 (0.72) |
| Pitch | 21.42 (8.91) | 32.12 (32.03) | 57.84 (0.73) |
7.3 Results
Across three selected cube rotations, ten repeated searches using 1,000 reduced-model samples each find configurations with errors around 10 mm and 30 mrad. Fewer samples may reduce cost but increase variability. Some opposite-pitch and roll motions remain physically difficult; this is not arbitrary-pose grasp synthesis.
8 Global Planning with Roadmaps
A reusable roadmap composes useful grasp configurations and their local transition plans. Offline construction searches for connections; online inference connects the requested endpoints and searches the resulting graph. See Choset et al. (2005) for the cited foundation.
8.1 Roadmap Construction
Each edge first moves the object with MPC, then repositions the hand with collision-free planning while the object stays fixed. Cube symmetry expands a few grasps and three basic rotations into 24 equivalent orientations, keeping this specialized roadmap below ten minutes of offline construction. On hardware, a random walk completed 150 consecutive transitions before the hand overheated. The authors attribute reliability partly to symmetry and grasps that cage and recenter the cube.
8.2 Inference on a Roadmap
Online planning connects the start and goal to nearby roadmap vertices, then finds a shortest graph path. The precomputed local transitions are reused, with endpoint connections solved for the new request. Success still depends on those connections being feasible.
8.3 Limitations
The roadmap is specialized to Allegro and a symmetric cube resting in stable palm-supported poses. General shapes require more explicit reachability reasoning, and unstable poses cannot be treated as static during finger repositioning. Finger gaiting or other stability-aware transit strategies would be needed to broaden applicability.
9 Discussion and Conclusion
The discussion separates the demonstrated model-based controller from possible uses in learning, then identifies unresolved contact-model and global-planning limitations.
9.1 Synergies with Learning-based Methods
The planner could complement learned policies by producing demonstrations, recovery trajectories, or useful exploration states. These are proposed interfaces to imitation and reinforcement learning rather than additional learning experiments established here.
9.1.1 Data Synthesis and Augmentation for IL
Potential imitation-learning uses include generating contact-rich demonstrations that are hard to teleoperate and recovering from perturbed demonstrations to enrich training data. Distilling such trajectories into a policy could reduce online planning cost. The discussion motivates this direction; it does not report a trained general policy as an outcome of these experiments. See Yang et al. (2025) for the cited foundation.
9.1.2 Efficient Exploration for RL
Stable roadmap states and successful trajectories could guide reinforcement-learning resets, rewards, or exploration. Prior planner-assisted learning work motivates this idea, but the claim that this roadmap is an efficient exploration distribution remains a hypothesis. See Khandate et al. (2024) for the cited foundation.
9.2 Concluding Remarks and Limitations
Local control succeeds on most sampled reachable goals, and a specialized roadmap extends its range. Remaining gaps include unexplained failures, incomplete understanding of primal versus dual feasibility, loss of contact under second-order dynamics, and dependence on stable symmetric roadmap nodes. Extension to other robots is plausible from supplied models, but may require added balance constraints and careful calibration; it has not been established universally.
Appendix A Appendix
The appendix supplies sensitivity and wrench-set derivations, goal sampling, hyperparameters, reduced-order grasp search, and additional failure/success examples.
A.1 Derivation of ( 15 )
Differentiating stationarity and perturbed complementarity produces the stated action-sensitivity relation. The appendix explicitly warns that this displayed relation alone is not invertible for higher-dimensional friction cones: the appropriate vector cone relation must be differentiated instead. The full matrix-calculus derivation is omitted, so the scalar-cone shortcut should not be implemented as a general friction-cone solver. See Magnus and Neudecker (2019) for the cited foundation.
Equations 45a–46b
| (45a) | ||||
| (45b) | ||||
| (46a) | ||||
| (46b) | ||||
A.2 Proof of Lemma 2
The equivalence proof uses the earlier first-order force-balance lemma: joint linear predictions of contact impulse and object displacement satisfy the same quasidynamic balance. It identifies the resulting motion construction under the stated local assumptions, rather than proving exact reachability for nonlinear physical contact. The appendix uses a shared actuator perturbation across contacts; its force/impulse and time-step conventions differ from the preceding wrench formulas, so the equations are preserved rather than silently normalized.
Equations 47a–52
| (47a) | ||||
| (47b) | ||||
| (47c) | ||||
| (47d) | ||||
| (48) |
| (49) |
| (50) | ||||
| (51) |
| (52) |
A.3 Goal Generation for Local MPC
Bimanual goals come from the boundary of a large local action-only motion set after sampling contacts and solving inverse kinematics; Allegro targets differ by about 60 degrees. The actual datasets contain 1,233 and 1,000 pairs respectively. Table 9 gives the no-motion baseline errors, clarifying the sampled goals’ scale and selection bias toward local reachability.
| IiwaBimanual | AllegroHand | |
|---|---|---|
| Mean [mm] | 152 | 16 |
| Mean [mrad] | 356 | 788 |
A.4 Effect of Planning Horizon for Local MPC
For the sampled bimanual goals, increasing the internal planning horizon from one to five barely changes final error. The proposed explanation is that quadratic goal distance approximates the cost-to-go well in this regime. This does not establish one-step optimality generally, especially for the nonlocal tasks motivating the roadmap.
| Translation [mm] | 2.02 | 1.99 | 1.64 | 2.11 | 2.10 |
|---|---|---|---|---|---|
| (3.10) | (4.38) | (3.14) | (3.79) | (3.17) | |
| Rotation [mrad] | 2.72 | 3.51 | 3.25 | 4.13 | 5.03 |
| (0.75) | (1.18) | (2.87) | (1.41) | (2.82) |
A.5 Hyperparameters for Local MPC
Object error and action regularization drive the cost; robot goal-coordinate weights are zero. Smoothing and time step trade informative gradients against model fidelity and execution length. Distant contact pairs are pruned using a conservative signed-distance threshold. More principled bounds could reduce the number of constraints further.
| IiwaBimanual | AllegroHand | ||
|---|---|---|---|
| Rotation | |||
| Translation | |||
| [s] | |||
| -threshold [m] | 0.2 | 0.05 |
A.6 Hyperparameters for 2nd-order Dynamics Stabilization
Second-order experiments tune rollout length and replanning count differently for tabletop and in-hand manipulation. The reported sweep shows almost no correlation between chunk length and final error. The robot pauses during replanning, so the chosen lengths also balance computation pauses against feedback frequency.
| (Number of re-plans) | (MPC rollout horizon) | |||
|---|---|---|---|---|
| System | open-loop | closed-loop | open-loop | closed-loop |
| IiwaBimanual | 1 | Variable | 25 | 5 |
| AllegroHand | 1 | 5 | 50 | 10 |
A.7 Heuristics for Generating Allegro Contact Configurations
Allegro grasp search first optimizes a simplified fingertip representation and then maps it to joint configurations. Sampling is biased toward actual surface contact to avoid ineffective separated fingertips.
A.7.1 A Reduced-Order Model
Four independently moving bounded spheres approximate the fingertips. Iterative quadratic-program inverse kinematics matches their optimized positions with the real hand while enforcing linearized non-penetration. This reduces search dimensionality but relies on the mapping being achievable with the hand’s kinematics.
Equations 53a–53d
| (53a) | ||||
| s.t. | (53b) | |||
| (53c) | ||||
| (53d) | ||||
A.7.2 Contact Sampling Distribution
Sampled sphere positions are projected onto the nearest cube surface. This concentrates candidates on contact configurations and increases the estimated robustness relative to unconstrained volume sampling.
A.8 Local MPC Examples
The additional trajectories contrast unilateral configurations, where ellipsoidal control can command fictitious pulling and fail, with stable force-closure grasps where both methods succeed. These examples support the mechanism attributed to dual feasibility while showing why its benefit varies with the initial contact geometry.