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{{}rawsensor\}\to {\mathfrak{W}}_t\to r_t\to g_t^{\{}\{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),{\nu }_t=(v_{\{}x,t\},v_{\{}y,t\},{\omega }_t).$$
4. #4

   Discrete-time kinematics (time step ):

   $$\begin{array}{rl}{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{array}$$
5. #5

   $$q_t^a=(q_{\{}1,t\},q_{\{}2,t\},q_{\{}3,t\},q_{\{}4,t\}),{\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^{\{}\{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,\{odom{\}}_t,q_t^a,{\dot{q}}_t^a,\{force{\}}_t,\dots ).$$
12. #12

    History encoder:

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

    Node/edge construction:

    $$V_t={\mathcal{N}}_{\varphi }(z_t),r_t(i,j)=(i,j,w_t(i,j),g_t(i,j)).$$
14. #14

    Embodiment-conditioned edge score (example):

    $$\begin{array}{rl}{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{array}$$
15. #15

    Given thresholds and :

    $$F\text{{}isfruit\}⟺\varphi (F)=\frac{\{}{|}\partial F{|}_w\}\{\{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 \},b_F(i)=d(i)-d_F^{\{}\{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 \underset{r\in \mathcal{R}}{\max }[{\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)].$$
21. #21

    World deficit:

    $${\Delta }_t={\lambda }_1\{StemCongestion\}+{\lambda }_2\{DoorOverload\}+{\lambda }_3\{BufferMismatch\}+{\lambda }_4\{HumanBurden\}+{\lambda }_5\{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^{\{}\{task\}\}\mapsto (g_t^{\{}\{nav\}\},g_t^{\{}\{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 }_{\{}\{motion\}\}(y_{\{}0:t\},{\mathfrak{W}}_t,r_t,g_t^{\{}\{task\}\}),{\xi }_t=({\xi }_t^b,{\xi }_t^a).$$
26. #26

    Coupled objective (horizon ):

    $$\underset{{\mathbf{x}}^b,{\mathbf{q}}^a,\mathbf{u}}{\min }\sum _{\tau =t}^{t+H}({\ell }_{\mathrm{task}}+{\lambda }_1{\ell }_{\mathrm{world}}+{\lambda }_2{\ell }_{\mathrm{reach}}+{\lambda }_3{\ell }_{\mathrm{safety}}+{\lambda }_4{\ell }_{\mathrm{energy}}).$$
27. #27

    where

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

    $${\ell }_{\mathrm{world}}={\gamma }_1\mathrm{dist}(x_{\tau }^b,{\mathcal{C}}_{\mathrm{stem}}{)}^2+{\gamma }_{2}door\_alignment(x_{\tau }^b,\Sigma )+{\gamma }_{3}boundary\_respect,$$
29. #29

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