Chapter 17 — End-to-End Mobile Manipulator

Open source document →

1. #1

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

   $$\text\{raw sensor\} \to \mathfrak W_t \to r_t \to g_t^\{\mathrm\{task\}\} \to \xi_t \to u_t.$$
2. #2

   In this embodiment, the robot is specialised as:

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

   $$x_t^b = (x_t, y_t, \theta_t), \qquad \nu_t = (v_\{x,t\}, v_\{y,t\}, \omega_t).$$
4. #4

   Discrete-time kinematics (time step ):

   $$\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}$$
5. #5

   $$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\}).$$
6. #6

   $$x_t^r = (x_t^b, q_t^a, \nu_t, \dot q_t^a).$$
7. #7

   $$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\}).$$
8. #8

   Let end-effector pose be

   $$p_t^e = f_{\mathrm{FK}}(x_t^b, q_t^a).$$
9. #9

   Then velocity is

   $$\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,$$
10. #10

    or compactly

    $$\dot p_t^e = J_{\mathrm{whole}}(x_t^r)\,\nu_t^{\mathrm{mb}}.$$
11. #11

    Sensor stream:

    $$y_t = (I_t, D_t, L_t, \mathrm\{odom\}_t, q_t^a, \dot q_t^a, \mathrm\{force\}_t, \ldots).$$
12. #12

    History encoder:

    $$z_t = E_\phi(y_{0:t}).$$
13. #13

    Node/edge construction:

    $$V_t = \mathcal N_\phi(z_t), \qquad r_t(i,j) = (i,j,w_t(i,j),g_t(i,j)).$$
14. #14

    Embodiment-conditioned edge score (example):

    $$\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}$$
15. #15

    Given thresholds and :

    $$F \text\{ is fruit\} \iff \phi(F) = \frac\{|\partial F|_w\}\{\mathrm\{vol\}(F)\} \le \theta,$$
16. #16

    $$S_t = V_t \setminus \bigcup_k F_k,$$
17. #17

    $$\Sigma(F)=\{i\in F : b_F(i)\ge\tau\}, \quad b_F(i)=d(i)-d_F^\{\mathrm\{int\}\}(i).$$
18. #18

    World summary:

    $$\mathfrak W_t = (\{F_k\}, S_t, \{\Sigma(F_k)\}).$$
19. #19

    Role family:

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

    Role selection:

    $$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].$$
21. #21

    World deficit:

    $$\Delta_t = \lambda_1\mathrm\{StemCongestion\} +\lambda_2\mathrm\{DoorOverload\} +\lambda_3\mathrm\{BufferMismatch\} +\lambda_4\mathrm\{HumanBurden\} +\lambda_5\mathrm\{IdleLoss\}.$$
22. #22

    Task generation:

    $$g_t^{\mathrm{task}} = \pi_{\mathrm{task}}(r_t,\mathfrak W_t,\Delta_t).$$
23. #23

    Each task is decomposed as

    $$g_t^\{\mathrm\{task\}\} \mapsto (g_t^\{\mathrm\{nav\}\}, g_t^\{\mathrm\{manip\}\}),$$
24. #24

    with canonical execution phases:

    $$\text{approach} \to \text{align} \to \text{manipulate} \to \text{exit}.$$
25. #25

    Motion latent:

    $$\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).$$
26. #26

    Coupled objective (horizon ):

    $$\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).$$
27. #27

    where

    $$\ell_\{\mathrm\{task\}\} = \|f_\{\mathrm\{FK\}\}(x_\tau^b,q_\tau^a)-p_\tau^\star\|^2,$$
28. #28

    $$\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},$$
29. #29

    $$\ell_\{\mathrm\{reach\}\} = \mathrm\{IK\_residual\}(x_\tau^b,q_\tau^a,p_\tau^\star)^2.$$