Skip to main content
Jaehong Oh

ℓPart 7· Chapter 17

Chapter 17 — End-to-End Mobile Manipulator

934 words4 min read

17.1 Purpose and Scope

This chapter instantiates RelationWorld theory for a practical mobile manipulator architecture:

  • Mobile base: 4-wheel swerve drive on the plane
  • Manipulator: 4-DOF arm
  • Sensors: RGB/depth camera, 2D/3D LiDAR, IMU, odometry, joint states, optional force/contact

The aim is not to replace Chapters 1--16, but to provide an embodiment-level map:

{rawsensor}→Wt→rt→gt{{task}}→ξt→ut.\text\{raw sensor\} \to \mathfrak W_t \to r_t \to g_t^\{\mathrm\{task\}\} \to \xi_t \to u_t.

17.2 Ontological Role of the Robot

In this embodiment, the robot is specialised as:

mobile manipulator=stem traversal+door manipulation.\text{mobile manipulator} = \text{stem traversal} + \text{door manipulation}.

Hence, its operational priority is not arbitrary in-fruit dexterity, but reliable traversal over stem regions and intervention at door boundaries.

17.3 State and Action Model

17.3.1 Base state (swerve)

xtb=(xt,yt,θt),νt=(v{x,t},v{y,t},ωt).x_t^b = (x_t, y_t, \theta_t), \qquad \nu_t = (v_\{x,t\}, v_\{y,t\}, \omega_t).

Discrete-time kinematics (time step Δt\Delta t):

xt+1=xt+(vx,tcos⁡θt−vy,tsin⁡θt)Δt,yt+1=yt+(vx,tsin⁡θt+vy,tcos⁡θt)Δt,θt+1=θt+ωtΔt.\begin{aligned} x_{t+1} &= x_t + (v_{x,t}\cos\theta_t - v_{y,t}\sin\theta_t)\Delta t, \\ y_{t+1} &= y_t + (v_{x,t}\sin\theta_t + v_{y,t}\cos\theta_t)\Delta t, \\ \theta_{t+1} &= \theta_t + \omega_t\Delta t. \end{aligned}

17.3.2 Arm state

qta=(q{1,t},q{2,t},q{3,t},q{4,t}),q˙ta=(q˙{1,t},q˙{2,t},q˙{3,t},q˙{4,t}).q_t^a = (q_\{1,t\},q_\{2,t\},q_\{3,t\},q_\{4,t\}), \qquad \dot q_t^a = (\dot q_\{1,t\},\dot q_\{2,t\},\dot q_\{3,t\},\dot q_\{4,t\}).

17.3.3 Combined robot state and action

xtr=(xtb,qta,νt,q˙ta).x_t^r = (x_t^b, q_t^a, \nu_t, \dot q_t^a). ut{{mb}}=(v{x,t},v{y,t},ωt,q˙{1,t},q˙{2,t},q˙{3,t},q˙{4,t}).u_t^\{\mathrm\{mb\}\} = (v_\{x,t\}, v_\{y,t\}, \omega_t, \dot q_\{1,t\}, \dot q_\{2,t\}, \dot q_\{3,t\}, \dot q_\{4,t\}).

17.4 Base-Arm Coupled Kinematics

Let end-effector pose be

pte=fFK(xtb,qta).p_t^e = f_{\mathrm{FK}}(x_t^b, q_t^a).

Then velocity is

p˙te=Jb(xtb,qta)νt+Ja(xtb,qta)q˙ta,\dot p_t^e = J_b(x_t^b,q_t^a)\nu_t + J_a(x_t^b,q_t^a)\dot q_t^a,

or compactly

p˙te=Jwhole(xtr) νtmb.\dot p_t^e = J_{\mathrm{whole}}(x_t^r)\,\nu_t^{\mathrm{mb}}.

This coupling equation is the mechanical core of the end-to-end formulation.

17.5 Sensor to Relational Field

Sensor stream:

yt=(It,Dt,Lt,{odom}t,qta,q˙ta,{force}t,…).y_t = (I_t, D_t, L_t, \mathrm\{odom\}_t, q_t^a, \dot q_t^a, \mathrm\{force\}_t, \ldots).

History encoder:

zt=Eϕ(y0:t).z_t = E_\phi(y_{0:t}).

Node/edge construction:

Vt=Nϕ(zt),rt(i,j)=(i,j,wt(i,j),gt(i,j)).V_t = \mathcal N_\phi(z_t), \qquad r_t(i,j) = (i,j,w_t(i,j),g_t(i,j)).

Embodiment-conditioned edge score (example):

wt(i,j)=  α1 proximityij+α2 handoverij+α3 cofuncij+α4 causalij+α5 manipulabilityij.\begin{aligned} w_t(i,j) = &\;\alpha_1\,\mathrm{proximity}_{ij} + \alpha_2\,\mathrm{handover}_{ij} + \alpha_3\,\mathrm{cofunc}_{ij} \\ &+ \alpha_4\,\mathrm{causal}_{ij} + \alpha_5\,\mathrm{manipulability}_{ij}. \end{aligned}

17.6 World Decomposition

Given thresholds θ∈(0,1)\theta \in (0,1) and τ>0\tau > 0:

F{isfruit}  ⟺  ϕ(F)={∣∂F∣w}{{vol}(F)}≤θ,F \text\{ is fruit\} \iff \phi(F) = \frac\{|\partial F|_w\}\{\mathrm\{vol\}(F)\} \le \theta, St=Vt∖⋃kFk,S_t = V_t \setminus \bigcup_k F_k, Σ(F)={i∈F:bF(i)≥τ},bF(i)=d(i)−dF{{int}}(i).\Sigma(F)=\{i\in F : b_F(i)\ge\tau\}, \quad b_F(i)=d(i)-d_F^\{\mathrm\{int\}\}(i).

World summary:

Wt=({Fk},St,{Σ(Fk)}).\mathfrak W_t = (\{F_k\}, S_t, \{\Sigma(F_k)\}).

17.7 Role Inference and Task Generation

Role family:

R={{stem−transporter},  {door−operator},  {door−stabilizer}}.\mathcal R = \{\text\{stem-transporter\},\; \text\{door-operator\},\; \text\{door-stabilizer\}\}.

Role selection:

rt=arg⁡max⁡r∈R[β1ΔFlow(r)+β2ΔBoundaryStability(r)+β3ΔManipulationUtility(r)−β4Cost(r)−β5Risk(r)].r_t = \arg\max_{r\in\mathcal R}\Big[ \beta_1\Delta\mathrm{Flow}(r) + \beta_2\Delta\mathrm{BoundaryStability}(r) + \beta_3\Delta\mathrm{ManipulationUtility}(r) -\beta_4\mathrm{Cost}(r) -\beta_5\mathrm{Risk}(r) \Big].

World deficit:

Δt=λ1{StemCongestion}+λ2{DoorOverload}+λ3{BufferMismatch}+λ4{HumanBurden}+λ5{IdleLoss}.\Delta_t = \lambda_1\mathrm\{StemCongestion\} +\lambda_2\mathrm\{DoorOverload\} +\lambda_3\mathrm\{BufferMismatch\} +\lambda_4\mathrm\{HumanBurden\} +\lambda_5\mathrm\{IdleLoss\}.

Task generation:

gttask=πtask(rt,Wt,Δt).g_t^{\mathrm{task}} = \pi_{\mathrm{task}}(r_t,\mathfrak W_t,\Delta_t).

17.8 Navigation-Manipulation Pairing

Each task is decomposed as

gt{{task}}↦(gt{{nav}},gt{{manip}}),g_t^\{\mathrm\{task\}\} \mapsto (g_t^\{\mathrm\{nav\}\}, g_t^\{\mathrm\{manip\}\}),

with canonical execution phases:

approach→align→manipulate→exit.\text{approach} \to \text{align} \to \text{manipulate} \to \text{exit}.

For the first benchmark family (door handle/tray), these phases are mandatory.

17.9 Motion Latent and Hybrid End-to-End Objective

Motion latent:

ξt=π{{motion}}(y{0:t},Wt,rt,gt{{task}}),ξt=(ξtb,ξta).\xi_t = \pi_\{\mathrm\{motion\}\}(y_\{0:t\},\mathfrak W_t,r_t,g_t^\{\mathrm\{task\}\}), \qquad \xi_t=(\xi_t^b,\xi_t^a).

Coupled objective (horizon HH):

min⁡xb,qa,u∑τ=tt+H(ℓtask+λ1ℓworld+λ2ℓreach+λ3ℓsafety+λ4ℓenergy).\min_{\mathbf x^b,\mathbf q^a,\mathbf u} \sum_{\tau=t}^{t+H} \Big( \ell_{\mathrm{task}} + \lambda_1\ell_{\mathrm{world}} + \lambda_2\ell_{\mathrm{reach}} + \lambda_3\ell_{\mathrm{safety}} + \lambda_4\ell_{\mathrm{energy}} \Big).

where

ℓ{{task}}=∥f{{FK}}(xτb,qτa)−pτ⋆∥2,\ell_\{\mathrm\{task\}\} = \|f_\{\mathrm\{FK\}\}(x_\tau^b,q_\tau^a)-p_\tau^\star\|^2, ℓworld=γ1 dist(xτb,Cstem)2+γ2 door_alignment(xτb,Σ)+γ3 boundary_respect,\ell_{\mathrm{world}} = \gamma_1\,\mathrm{dist}(x_\tau^b,\mathcal C_{\mathrm{stem}})^2 +\gamma_2\,\mathrm{door\_alignment}(x_\tau^b,\Sigma) +\gamma_3\,\mathrm{boundary\_respect}, ℓ{{reach}}={IK_residual}(xτb,qτa,pτ⋆)2.\ell_\{\mathrm\{reach\}\} = \mathrm\{IK\_residual\}(x_\tau^b,q_\tau^a,p_\tau^\star)^2.

17.10 ROS 2 Interface Contract

Implementation-facing message/action contracts are standardised in package antbot_relational_e2e:

  • RelationalWorld.msg
  • RoleAssignment.msg
  • GeneratedTask.msg
  • DoorManip.action

This contract is the bridge between theory-level variables and executable ROS graphs.

17.11 Traceability to Core Theory

Core theory (Ch.1--16)Embodiment mapping (this chapter)
Relation tuple and weighted fieldSensor-conditioned graph features
Fruit/stem/door decompositionWorkspace partition and door intervention points
Theorems A--H (stability and structure)Constraints and penalties in world-consistency objective
Existence and world readingRole assignment + task generation pipeline
Dynamics (Ch.14)Online update of world graph and role/task refresh

17.12 Validation Gates

To move from specification to simulation:

  1. Dimensional consistency of state/action/kinematics equations.
  2. Interface completeness for world, role, task, and manipulation action.
  3. Task closure: door-handle/tray scenario must complete all four phases.
  4. Objective traceability: each objective term mapped to at least one theoretical primitive.

These gates define readiness for the simulation implementation phase.