CUHK Collaboration: Mobile Dual-Arm Manipulation for Library Shelf Tidying and Book Reshelving

In semi-structured physical environments, returning a book to a shelf appears deceptively simple. For an autonomous mobile dual-arm robot, however, it presents severe multi-physics challenges spanning spatial geometry, contact dynamics, and long-horizon decision-making:

• Books on a library shelf are packed tightly side by side, subject to substantial lateral compression and surface friction, leaving clearances often strictly in the range of 1 to 2 millimeters;
• Book spines exhibit varied textures and reflective laminated plastic coverings, frequently inducing orientation estimation errors in optical 6D pose estimators;
• If the mobile base stops merely a few centimeters off-target, the arm rapidly hits joint singularities or mechanical limits when reaching deep into the shelf slot, or occludes the optical line-of-sight of wrist-mounted cameras;
• Extracting or inserting a book without lateral force regulation easily triggers a domino-style collapse of neighboring leaned volumes.

This project is an ongoing research collaboration with Prof. Yunhui Liu’s team at The Chinese University of Hong Kong (CUHK). We focus on mobile dual-arm manipulation for autonomous library inventory, shelf reshelving, and tidying. I lead the AI Agent decision-making and physical execution loop, investigating how high-level tidying goals translate into reliable, verifiable physical actions.

Project context

My contributions

As project lead, established the mobile dual-arm manipulation framework; responsible for coaxial visual servoing and closed-loop agent execution, setting research targets for bimanual extraction (>92% success rate) and jammed slot recovery (>80% recovery rate) in 1.5–3.0 mm tight shelf clearances.

Physical Challenges and the Fundamental Paradigm Shift of Mobile Manipulation

Transitioning dual-arm manipulation from fixed industrial workcells to a mobile wheeled platform represents a fundamental paradigm shift in both theoretical formulation and physical execution. In a fixed workcell, an unobserved object is typically a localized line-of-sight occlusion problem; the arm base is rigidly anchored to the workstation, and inverse kinematics (IK) possesses a static, deterministic geometric reachability envelope. In mobile manipulation, however, the problem expands across three critical physical dimensions:

1. Environment-Scale Partial Observability: The target book may reside on an entirely different shelf rack, in an adjacent aisle, or placed in a temporary return cart by human visitors. The robot must not only infer localized occlusions within its current view frustum, but must also perform global spatial reasoning across rooms and aisles, planning active exploratory navigation paths while maintaining a persistent topological and object-centric memory graph.
2. Base Placement as a First-Class Decision: The terminal parking pose of the mobile base directly dictates the kinematic feasibility of all subsequent manipulation motions. Stopping too close to the shelf prevents the arm from unfurling its elbow joints, risking collisions with narrow aisle walls behind the robot; stopping too far leaves deep shelf slots unreachable. Furthermore, the base yaw angle directly constraints the line-of-sight of onboard cameras toward spine boundaries and target slot gaps. Base docking can never be relegated to an independent 2D navigation stack; it must be treated as a first-class decision variable driven by predicted manipulation payoff.
3. Uncertainty Accumulation across Navigation-Manipulation Cycles: After executing a long transit path and braking to a halt, wheel odometry slippage, mechanical gear backlash, and chassis suspension compliance introduce residual base pose errors on the order of 1 to 3 centimeters and several degrees in heading. Executing pre-planned arm trajectories open-loop from global navigation coordinates inevitably results in hard physical collisions. The system must enforce a deterministic sequence of secondary local perception, pose refinement, and compliant force-closed feedback.

Software-Hardware Division and the Deterministic Control Boundary

To deploy complex AI reasoning onto physical hardware safely, our architecture establishes an uncompromising division of responsibilities: the AI Agent and the World Model operate strictly at the task and skill level, governing high-level discrete state transitions, base parking candidates, verification scheduling, and bounded recovery policies. Low-level trajectory interpolation, kinematics, collision checking, torque limits, and emergency stopping are enforced deterministically by the robot control stack.

• High-Level Agent Planning System: Implemented within the ROS 2 ecosystem, running a dedicated local language model adapter (agent_planner). It consumes the structured symbolic task state, registered skill definitions, and the object-centric world model, outputting structured plans strictly adhering to predefined JSON schema declarations.
• Low-Level Deterministic Control Stack: Built upon ROS 2 and MoveIt 2 for kinematic computation and planning scenes. Free-space arm motions utilize OMPL for collision-free trajectory generation, while slot insertion and retraction strictly invoke Cartesian straight-line interpolation. Mobile base navigation relies on Nav2 maintaining layered costmaps, coordinated with an omnidirectional mobile chassis to provide smooth translational and rotational braking.
• Onboard Compute and Real-Time Bus: Computation is hosted on an onboard industrial GPU workstation running perception segmentation, world model maintenance, local trajectory synthesis, and skill status monitoring. Low-level motor controllers communicate with arm joints and base wheel drives over high-speed real-time Ethernet buses, guaranteeing millisecond-level emergency stops whenever an anomalous wrench or deviation is detected.

Joint Optimization of Base Docking and Arm Manipulability

In narrow library aisles where aisle clearance is typically restricted to 0.8 to 1.0 meters, decoupling mobile base parking from manipulator trajectory generation causes severe kinematic deadlocks. We formulate base pose selection as a multi-objective constrained optimization problem, jointly considering arm manipulability (steering clear of singularities and travel limits), wrist camera line-of-sight margins (preventing link self-occlusion), and navigation travel and safety clearances.

Once the base completes navigation and comes to a complete halt, the robot triggers a two-stage local visual refinement routine: onboard stereo and wrist cameras reconstruct local shelf fiducials and target spine edges, computing an incremental correction matrix that absorbs the 1-3 cm base drift before any arm motion commences.

Coordinated Bimanual Manipulation and Compliant Contact Mechanics

The most fragile phase of library shelf manipulation lies in the lateral force interactions during book extraction and slot re-insertion. Under tight packing, pulling a book out with a single arm drags adjacent leaning volumes through surface friction, provoking domino-style collapses. Conversely, during insertion, even sub-degree angular misalignment locks the book against slot edges. We address this through a specialized bimanual strategy.

1. Stabilizing Arm: Lateral Preloading and Anti-Collapse Support

Prior to any contact between the primary manipulator and the target book, the secondary stabilizing arm extends and positions a compliant, high-friction pad against the outer edge of the adjacent leaning book cluster, applying a regulated lateral preload (typically maintained at 5 N to 8 N):

• This steady normal force functions as an artificial rigid sidewall, counteracting lateral friction forces induced when the target volume slides out;
• A 6-axis force-torque sensor at the stabilizing wrist continuously tracks lateral reaction forces. If anomalous cluster slippage is detected, the controller adaptively adjusts contact depth to preserve the quasi-static equilibrium of the shelf.

2. Primary Manipulating Arm: Tangential Compliant Extraction

The primary arm is equipped with a slim, adaptive parallel-jaw gripper. Perception algorithms locate the upper and lower spine vertices alongside the surface normal, guiding the gripper tips to slide tangentially into the narrow book gaps:

• Gripper closing combines motor current sensing with optical displacement feedback, clamping the book spine firmly without crushing fragile paper covers;
• Extraction proceeds along the shelf normal. The manipulator operates in an admittance-controlled mode along lateral (Y) and pitch axes, allowing minor passive compliance to accommodate slight alignment errors without tearing delicate book jackets.

3. Tilted Micro-Search and Compliant Slot Insertion

Re-inserting a book into an ultra-narrow slot represents the highest contact-dynamic hurdle. We implement a three-phase insertion protocol:

• Tilted Initial Contact: The bottom corner of the book is tilted at a slight angle of 12 to 15 degrees, reducing the initial contact geometry from an area contact to an edge-point contact, substantially diminishing sliding resistance;
• Admittance Control and Compliant Search: In the Cartesian task frame, the manipulator tracks a compliant admittance model. The forward insertion axis is parameterized with low virtual stiffness, automatically slowing down upon sudden thrust increases while applying subtle oscillatory dither motions to guide the book along the natural boundaries of the slot;
• Full Home Seating: When reaction forces drop to nominal levels and optical distance sensors confirm the spine is flush with the shelf plane, the gripper releases smoothly and retracts.

Object-Centric Shelf World Model

Maintaining the physical state of dozens of individual books across an aisle exceeds the capability of raw dense voxel grids or unstructured point clouds, which incur prohibitive computational overhead and lack semantic handles. We developed an object-centric shelf world model specifically tailored for dense manipulation.

1. Structured Node Representations

The world model represents the environment as an explicit geometric-semantic heterogeneous graph. Each node corresponds to an individual book entity or shelf partition unit:

• Spatial Geometry: 3D oriented bounding boxes (3D OBB), 6D spine reference frame, and posterior pose covariance $\Sigma_p$;
• Semantic Identity: Optical character recognition (OCR) spine text, barcode/RFID identifiers, and visual feature embeddings;
• Topology and Contact: Left neighbor ID, right neighbor ID, supporting shelf plane, tilt angle $\phi_{\text{tilt}}$, and lateral tightness metric.

2. Multi-View Spatial-Temporal Data Association

As the mobile base traverses the aisles, incoming point clouds and RGB streams are continuously associated with existing graph nodes via Hungarian matching over a cost matrix combining 3D Euclidean proximity, visual appearance similarity, and relative topological sequence. When an existing book is re-observed from a different vantage point, a Kalman filtering pipeline updates its 3D pose estimate and reduces uncertainty ellipses. When a previously occupied coordinate is observed to be empty, the model updates the node state to a vacant slot.

3. Counterfactual Simulation for Action Safety

The world model enables the AI Agent to conduct counterfactual forward lookahead before executing physical actions:

• Evaluating whether the manipulator’s swept volume will violate collision margins with shelf uprights from a candidate base pose;
• Simulating whether extracting a target volume will destabilize adjacent volumes based on gravity vectors and friction coefficients;
• If counterfactual analysis indicates collapse risk, the agent proactively generates a coordinated bimanual plan incorporating the stabilizing arm rather than attempting single-arm manipulation.

AI Agent Task Orchestration and Bounded Error Recovery

As lead of the AI Agent workstream, I developed a unified task abstraction layer bridging high-level semantic reasoning with low-level execution via Behavior Trees (BehaviorTree.CPP) and parameterized skill contracts.

1. Structured Task Planning and Schema Validation

When presented with open-ended natural language instructions (such as “Retrieve three misplaced computer science books from the return trolley, place them onto Shelf B2, and tidy the leaning stack”):

• The onboard agent_planner digests the symbolic state from the world model (including robot region, shelf vacancies, and trolley inventory) through a local language model, decomposing the directive into a directed acyclic task graph;
• Action outputs are restricted to registered skills: NavigateToArea, DockAtShelf, ScanSpines, StabilizeCluster, ExtractBook, VerifyHolding, InsertBook, RetractArm;
• A deterministic schema validator verifies that all parameters, coordinate frames, and target IDs are mathematically sound and physically present before generating the reactive behavior tree.

2. Explicit Physical Verification Contracts

To avoid catastrophic cascading failures caused by open-loop assumptions, each atomic skill is bound by strict sensor-driven preconditions and postconditions:

• For ExtractBook, preconditions require that the stabilizing arm has verified normal force balance and the gripper is aligned with the spine axis;
• Postconditions require multi-modal verification: optical beam interruption on the gripper fingers combined with finger encoder positions confirming that the gripped width corresponds to the expected book thickness. If verification fails, the skill returns an explicit failure state, halting forward progression.

3. Multi-Tier Bounded Error Recovery

Mechanical binding during slot insertion is an inherent physical phenomenon due to paper curvature and localized friction. We embed bounded recovery routines:

• Local Micro-Retreat: If insertion force exceeds safety limits (such as thrust exceeding 25 N) prior to target depth, the behavior tree triggers a local recovery branch: retracting 20 mm, applying a 1.5-degree yaw adjustment, and re-attempting compliant insertion;
• Bounded Escalation and Safe Abort: If three consecutive recovery attempts encounter persistent jamming, the system terminates insertion, pulls the book back safely, transfers it to the onboard carriage, marks the slot as jammed in the world model, logs the anomaly, and proceeds to the next item, preventing a localized mechanical failure from paralyzing the overarching mission.

Progressive Milestones & Projected Performance Targets

The framework is architected across four difficulty milestones (M1 to M4):

• M1: Meeting-Room Clearing. Validating natural language instruction parsing, reachability-driven docking, basic single- and dual-arm manipulation, and onboard carriage payload handling in open environments;
• M2: Library Shelf Tidying and Reshelving. Introducing narrow aisle navigation, dense book extraction under compression, millimeter-clearance compliant insertion, bimanual stabilization, and jammed-slot recovery;
• M3: Traceable High-Value Transport. Adding sealed container handling, delicate vial grasping, barcode/RFID verification, and audit logging;
• M4: Unseen Open-World Generalization. Evaluating long-horizon stability and object-centric topological reasoning across novel room configurations without prior maps.

For M2 dense shelf tidying simulation and experimental benchmarking, the project defines concrete target objectives:

• In dense shelf arrays with clearances between 1.5 and 3.0 mm, coordinated bimanual extraction targets an overall success rate exceeding 92%, aiming to prevent neighbor domino collapses;
• Base docking optimization targets placing the manipulator in high-dexterity operating envelopes on >88% of initial attempts, minimizing realignment latency;
• Under induced binding disturbances, bounded error recovery is designed to resolve over 80% of frictional jams without human intervention.

Engineering Lessons and Technical Takeaways

This project provided deep practical insights into the physical realities of mobile dual-arm manipulation in dense environments:

1. The Inviolable Role of Deterministic Control: While modern end-to-end vision-language-action models offer broad generalization, their high-frequency output jitter remains hazardous during tight-clearance physical contact. Confining AI models to discrete task planning and counterfactual reasoning while delegating low-level execution to classical admittance control, OMPL, and behavior trees provides the necessary foundation for real-world hardware reliability.
2. Mobility and Manipulation Must Be Unified: A mobile manipulator is fundamentally more than a robotic arm mounted on a mobile platform. Treating base docking as an independent navigation waypoint is the primary cause of kinematic singularities and visual occlusions. Deriving optimal base docking manifolds directly from manipulation constraints translates mobility into an active extension of the arm’s workspace.
3. Physical Robustness Arises from Rigorous Error Defense: In long-horizon real-world operations, expecting zero physical failures is unrealistic. True system resilience lies not in preventing all errors, but in verifying physical states against reality and executing bounded local recovery when anomalies occur. The architectural paradigms developed here provide enduring foundations for dense shelf handling, semi-structured mobile manipulation, and embodied robotic systems.