Algorithm 1 Computing final configuration from (i) knowledge of initial configuration and (ii) wrench in final configuration using information from hand h.
Require:${\alpha }_{h0}$, $[{\mathit{r}}_3,{\theta }_3]{|}_{t_0}$, $[{\mathit{r}}_h,{\theta }_h]{|}_{t_0,t}$ and $[{\mathit{f}}_h^{\prime },{\tau }_h^{\prime }]{|}_t$	▹ (’) denotes local frame measurements
${\alpha }_{ht}\leftarrow {\alpha }^{*}\left([{\mathit{f}}_h^{\prime },{\tau }_h^{\prime }]{|}_t\right)$
$\Delta {\alpha }_h\leftarrow {\alpha }_{ht}-{\alpha }_{h0}$
$Rot\leftarrow \left[\begin{array}{cc}\mathit{R}(\Delta {\alpha }_h)& \mathbf{0}\\ \mathbf{0}& 1\end{array}\right]$	▹ Rotation about hand
$Trn\leftarrow \left[\begin{array}{cc}{\mathbb{I}}_2& -r_0\Delta {\alpha }_h{\mathit{t}}^{\prime }\left({\alpha }_{ht}\right)\\ \mathbf{0}& 1\end{array}\right]$	▹ Back-shifting along tangent
${\mathrm{\Phi }}_{30}\leftarrow {[{\mathit{r}}_3,{\theta }_3]}_{SE2}{{|}_{t_0},{\mathrm{\Phi }}_{h0}\leftarrow {[{\mathit{r}}_h,{\theta }_h]}_{SE2}|}_{t_0}$	▹ Initial state
${\mathrm{\Phi }}_{30}^{\prime }\leftarrow {\mathrm{\Phi }}_{h0}^{-1}·{\mathrm{\Phi }}_{30}$
${\mathrm{\Phi }}_{3t}^{\prime }\leftarrow Trn·Rot·{\mathrm{\Phi }}_{30}^{\prime }$	▹ Final state
${\mathrm{\Phi }}_{ht}\leftarrow {[{\mathit{r}}_h,{\theta }_h]}_{SE2}{|}_t$
${\mathrm{\Phi }}_{3t}\leftarrow {\mathrm{\Phi }}_{ht}·{\mathrm{\Phi }}_{3t}^{\prime }$
return $[{\mathit{r}}_3,{\theta }_3]{|}_t$