Commit c07fc22a by Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr

### [slides] Several improvements

  add a figure about relative poses,
improve exercise 2,
unify notations.
parent ee2eb48e
 ... @@ -105,8 +105,9 @@ Exercise 2 ... @@ -105,8 +105,9 @@ Exercise 2 Exercise 2.1 Exercise 2.1 ~~~~~~~~~~~~ ~~~~~~~~~~~~ In order to help the manipulation planner, define a constraint graph with In order to help the manipulation planner, define in file intermediate states like for instance: +grasp_ball_in_box.py+ a constraint graph with intermediate states like for instance: - a state where the gripper is empty above the ball - a state where the gripper is empty above the ball - a state where the gripper holds the ball above the ground. - a state where the gripper holds the ball above the ground. ... ...
 "http://www.w3.org/TR/xhtml11/DTD/xhtml11.dtd"> Manipulation planning Manipulation planning
 ... @@ -6,4 +6,7 @@ ... @@ -6,4 +6,7 @@ \begin {center} \begin {center} \includegraphics [width=.5\linewidth] {figures/be.png} \includegraphics [width=.5\linewidth] {figures/be.png} \end {center} \end {center} \begin{itemize} \item \texttt{script/grasp\_ball.py} \end{itemize} \end {frame} \end {frame}
No preview for this file type
 %% Creator: Inkscape inkscape 0.92.3, www.inkscape.org %% PDF/EPS/PS + LaTeX output extension by Johan Engelen, 2010 %% Accompanies image file 'relative-pose.pdf' (pdf, eps, ps) %% %% To include the image in your LaTeX document, write %% \input{.pdf_tex} %% instead of %% \includegraphics{.pdf} %% To scale the image, write %% \def\svgwidth{} %% \input{.pdf_tex} %% instead of %% \includegraphics[width=]{.pdf} %% %% Images with a different path to the parent latex file can %% be accessed with the `import' package (which may need to be %% installed) using %% \usepackage{import} %% in the preamble, and then including the image with %% \import{}{.pdf_tex} %% Alternatively, one can specify %% \graphicspath{{/}} %% %% For more information, please see info/svg-inkscape on CTAN: %% http://tug.ctan.org/tex-archive/info/svg-inkscape %% \begingroup% \makeatletter% \providecommand\color[2][]{% \errmessage{(Inkscape) Color is used for the text in Inkscape, but the package 'color.sty' is not loaded}% \renewcommand\color[2][]{}% }% \providecommand\transparent[1]{% \errmessage{(Inkscape) Transparency is used (non-zero) for the text in Inkscape, but the package 'transparent.sty' is not loaded}% \renewcommand\transparent[1]{}% }% \providecommand\rotatebox[2]{#2}% \newcommand*\fsize{\dimexpr\f@size pt\relax}% \newcommand*\lineheight[1]{\fontsize{\fsize}{#1\fsize}\selectfont}% \ifx\svgwidth\undefined% \setlength{\unitlength}{494.67450396bp}% \ifx\svgscale\undefined% \relax% \else% \setlength{\unitlength}{\unitlength * \real{\svgscale}}% \fi% \else% \setlength{\unitlength}{\svgwidth}% \fi% \global\let\svgwidth\undefined% \global\let\svgscale\undefined% \makeatother% \begin{picture}(1,0.80562299)% \lineheight{1}% \setlength\tabcolsep{0pt}% \put(0,0){\includegraphics[width=\unitlength,page=1]{relative-pose.pdf}}% \put(0.17477689,0.34063648){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}$T_1$\end{tabular}}}}% \put(0.44259931,0.46059872){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}$T_2$\end{tabular}}}}% \put(0,0){\includegraphics[width=\unitlength,page=2]{relative-pose.pdf}}% \put(0.54593367,0.65844625){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}{\color{red}$\mathbf{u}$}\end{tabular}}}}% \put(0,0){\includegraphics[width=\unitlength,page=3]{relative-pose.pdf}}% \put(0.65609859,0.28745037){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}{\color{red}$\mathbf{v}$}\end{tabular}}}}% \put(0.42206309,0.00667914){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}$R_1$\end{tabular}}}}% \put(0.62395352,0.05862306){\color[rgb]{0,0,0}\makebox(0,0)[lt]{\smash{\begin{tabular}[t]{l}$R_2$\end{tabular}}}}% \end{picture}% \endgroup%
 ... @@ -366,6 +366,10 @@ Motion constraints ... @@ -366,6 +366,10 @@ Motion constraints \begin {align*} \begin {align*} \forall t\in [0,1], f(\bar{\steer} (t)) &= 0 \forall t\in [0,1], f(\bar{\steer} (t)) &= 0 \end {align*} \end {align*} \pause It can be defined by projection $$\bar{\steer} = proj\circ \steer$$ \end {frame} \end {frame} \begin {frame} {Projecting path on constraint} \begin {frame} {Projecting path on constraint} ... @@ -521,12 +525,12 @@ Motion constraints ... @@ -521,12 +525,12 @@ Motion constraints \item <2-> for each connected component: \item <2-> for each connected component: \setlength\leftmargin{0em} \setlength\leftmargin{0em} \begin{itemize}[leftmargin=*] \begin{itemize}[leftmargin=*] \item[]<2-> $\conf_{near}$ = nearest\_neighbour($\conf_{rand}$, $roadmap$) \item[]<2-> $\conf_{near}$ = nearest\_neighbor($\conf_{rand}$, $roadmap$) \item[]<3-> $e$ = select\_transition($\conf_{near}$) \item[]<3-> $T$ = select\_transition($\conf_{near}$) \item[]<4-> $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $e$) \item[]<4-> $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $T$) \item[]<5-> $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, edge) \item[]<5-> $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, $T$) \item[]<6-> $roadmap$.insert\_node($\conf_{new}$) \item[]<6-> $roadmap$.insert\_node($\conf_{new}$) \item[]<7-> $roadmap$.insert\_edge(e, $\conf_{near}$, $\conf_{new}$) \item[]<7-> $roadmap$.insert\_edge($T$, $\conf_{near}$, $\conf_{new}$) \item[]<7-> new\_nodes.append ($\conf_{new}$) \item[]<7-> new\_nodes.append ($\conf_{new}$) \end{itemize} \end{itemize} \item <8-> for $\conf\in (\conf_{new}^1, ..., \conf_{new}^{n_{cc}})$: \item <8-> for $\conf\in (\conf_{new}^1, ..., \conf_{new}^{n_{cc}})$: ... @@ -544,10 +548,10 @@ Motion constraints ... @@ -544,10 +548,10 @@ Motion constraints \begin {frame} {Select transition} \begin {frame} {Select transition} $e$ = select\_transition($\conf_{near}$) $T$ = select\_transition($\conf_{near}$) \vskip .5cm \vskip .5cm Outward edges of each node are given a probability distribution. The transition from a node to another node is chosen by random sampling. Outward transitions of each state are given a probability distribution. The transition from a state to another state is chosen by random sampling. \begin{center} \begin{center} \begin{tikzpicture}[>=stealth',auto,node distance=3.5cm, \begin{tikzpicture}[>=stealth',auto,node distance=3.5cm, thick,main node/.style={circle,draw,text width=1.5cm,align=center,font=\footnotesize}] thick,main node/.style={circle,draw,text width=1.5cm,align=center,font=\footnotesize}] ... @@ -567,12 +571,12 @@ Motion constraints ... @@ -567,12 +571,12 @@ Motion constraints \begin {frame} {Generate target configuration} \begin {frame} {Generate target configuration} $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $e$) $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $T$) \vskip .5cm \vskip .5cm Once edge $e$ has been selected, $\conf_{rand}$ is \textit {projected} onto the destination node $n_{dest}$ in a configuration reachable by $\conf_{near}$. Once transition $T$ has been selected, $\conf_{rand}$ is \textit {projected} onto the destination state $S_{dest}$ in a configuration reachable by $\conf_{near}$. \begin{align*} \begin{align*} f_{e} (\conf_{proj}) &= f_{e} (\conf_{near})\\ f_{T} (\conf_{proj}) &= f_{T} (\conf_{near})\\ f_{dest} (\conf_{proj}) &= 0 f_{S_{dest}} (\conf_{proj}) &= 0 \end{align*} \end{align*} \end {frame} \end {frame} ... @@ -582,9 +586,9 @@ Once edge $e$ has been selected, $\conf_{rand}$ is \textit {projected} onto the ... @@ -582,9 +586,9 @@ Once edge $e$ has been selected, $\conf_{rand}$ is \textit {projected} onto the \begin {frame} {Extend} \begin {frame} {Extend} $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, edge) $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, $T$) \vskip.5cm \vskip.5cm \textit {Project} straight path $[\conf_{near},\conf_{proj}]$ on edge constraint: \textit {Project} straight path $[\conf_{near},\conf_{proj}]$ on transition constraint: \begin {itemize} \begin {itemize} \item if projection successful and projected path collision free \item if projection successful and projected path collision free  ... @@ -595,7 +599,7 @@ $$... @@ -595,7 +599,7 @@$$ \end {itemize} \end {itemize} \pause \pause  \forall \conf\in(\conf_{near},\conf_{new}),\ f_{e} (\conf) = f_{e}(\conf_{near}) \forall \conf\in(\conf_{near},\conf_{new}),\ f_{T} (\conf) = f_{T}(\conf_{near})  \end {frame} \end {frame} ... @@ -626,14 +630,14 @@ $$... @@ -626,14 +630,14 @@$$ \begin {frame} {Connect} \begin {frame} {Connect} connect ($\conf_0$, $\conf_1$): connect ($\conf_0$, $\conf_1$): \begin{itemize} \begin{itemize} \item [] $s_0$ = state ($\conf_0$) \item [] $S_0$ = state ($\conf_0$) \item [] $s_1$ = state ($\conf_1$) \item [] $S_1$ = state ($\conf_1$) \item [] e = transition ($n_0$, $n_1$) \item [] $T$ = transition ($S_0$, $S_1$) \item [] if e and $f_{e} (\conf_0) == f_{e} (\conf_1)$: \item [] if $T$ and $f_{T} (\conf_0) == f_{T} (\conf_1)$: \begin {itemize} \begin {itemize} \item [] if p = projected\_path (e, $\conf_0$, $\conf_1$) collision-free: \item [] if p = projected\_path ($T$, $\conf_0$, $\conf_1$) collision-free: \begin {itemize} \begin {itemize} \item [] roadmap.insert\_edge (e, $\conf_0$,$\conf_1$) \item [] roadmap.insert\_edge ($T$, $\conf_0$,$\conf_1$) \end {itemize} \end {itemize} \end {itemize} \end {itemize} \item [] return \item [] return ... @@ -723,17 +727,17 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. ... @@ -723,17 +727,17 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. \begin {columns} \begin {columns} \column {.6\linewidth} \column {.6\linewidth} $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $e$) $\conf_{proj}$ = generate\_target\_config($\conf_{near},\conf_{rand}$, $T$) \vskip .5cm \vskip .5cm $\conf_1\leftarrow$ pick configuration $\conf_1\leftarrow$ pick configuration \begin {itemize} \begin {itemize} \item in node $N_1$, \item in state $S_1$, \item not in same connected component as $\conf_{near}$ \item not in same connected component as $\conf_{near}$ \end {itemize} \end {itemize} \begin{align*} \begin{align*} f_{e_1} (\conf_{proj}) &= f_{e_1} (\conf_{near})\\ f_{T_1} (\conf_{proj}) &= f_{T_1} (\conf_{near})\\ {\color{red} f_{e_2} (\conf_{proj})} &{\color{red}= f_{e_2} (\conf_1)}\\ {\color{red} f_{T_2} (\conf_{proj})} &{\color{red}= f_{T_2} (\conf_1)}\\ f_{N_2} (\conf_{proj}) &= 0 f_{S_2} (\conf_{proj}) &= 0 \end{align*} \end{align*} \column {.4\linewidth} \column {.4\linewidth} \begin{center} \begin{center} ... @@ -741,12 +745,12 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. ... @@ -741,12 +745,12 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. \begin{tikzpicture}[>=stealth',auto,node distance=3.25cm, \begin{tikzpicture}[>=stealth',auto,node distance=3.25cm, thick,main node/.style={circle,draw,text width=1.25cm,align=center, thick,main node/.style={circle,draw,text width=1.25cm,align=center, font=\tiny}] font=\tiny}] {\node[main node] (nh) {$N_1$};} {\node[main node] (nh) {$S_1$};} {\node[main node] (h) [right of=nh] {$N_2$};} {\node[main node] (h) [right of=nh] {$S_2$};} {\path[<-] (h) edge[bend right=0] node[above] {Crossed foliation} {\path[<-] (h) edge[bend right=0] node[above] {Crossed foliation} (nh);} (nh);} {\path[<-] (h) edge[bend right=22.5] node[above] {$e_1$} (nh);} {\path[<-] (h) edge[bend right=22.5] node[above] {$T_1$} (nh);} {\path[->] (h) edge[bend left=22.5] node[below] {$e_2$} (nh);} {\path[->] (h) edge[bend left=22.5] node[below] {$T_2$} (nh);} \end{tikzpicture} \end{tikzpicture} \end{center} \end{center} \end {columns} \end {columns} ... @@ -758,9 +762,9 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. ... @@ -758,9 +762,9 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. \begin {frame} {Crossed foliation transition: extend} \begin {frame} {Crossed foliation transition: extend} $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, $e_1$) $\conf_{new}$ = extend($\conf_{near}$, $\conf_{proj}$, $T_1$) \vskip.5cm \vskip.5cm \textit {Project} straight path $[\conf_{near},\conf_{proj}]$ on $e_1$ constraint: \textit {Project} straight path $[\conf_{near},\conf_{proj}]$ on $T_1$ constraint: \begin {itemize} \begin {itemize} \item if projection successful and projected path collision free \item if projection successful and projected path collision free  ... @@ -768,10 +772,10 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. ... @@ -768,10 +772,10 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$.  \pause \pause \begin{align*} \begin{align*} f_{e_2} (\conf_{2}) &= f_{e_2} (\conf_1)\\ f_{T_2} (\conf_{2}) &= f_{T_2} (\conf_1)\\ f_{N_2} (\conf_{2}) &= 0 f_{S_2} (\conf_{2}) &= 0 \end{align*} \end{align*} \item $\conf_2$ is connectable to $\conf_1$ via $e_2$. \item $\conf_2$ is connectable to $\conf_1$ via $T_2$. \end{itemize} \end{itemize} \end {frame} \end {frame} ... @@ -781,18 +785,30 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. ... @@ -781,18 +785,30 @@ Manipulation RRT is initialized with $\conf_{init}$, $\conf_{goal}$. \begin {frame} {Relative positions as numerical constraints} \begin {frame} {Relative positions as numerical constraints} Let $T_1 = T_{(R_1,t_1)}$ and $T_2 = T_{(R_2,t_2)}$ be two rigid-body transformations. The relative transformation $T_{2/1} = T_1^{-1}\circ T_2$ can be represented by a vector of dimension 6: \centerline{ $$\parbox{.5\linewidth}{ \left(\begin{array}{c} \mathbf{u} \\ \mathbf {v}\end{array}\right) \begin{itemize}$$ \item $T_1 = T_{(R_1,t_1)}\in SE(3)$, $T_2 = T_{(R_2,t_2)}\in SE(3)$. where \item $T_{2/1} = T_1^{-1}\circ T_2$ can be represented by a vector of dimension 6: \begin {columns} $$\column {.5\textwidth} \left(\begin{array}{c} \mathbf{u} \\ \mathbf {v}\end{array}\right)$$ $$\mathbf{u} = R_1^T (t_2-t_1) where$$ $$\column {.5\textwidth} \mathbf{u} = R_1^T (t_2-t_1) R_1^T R_2 is the matrix of the rotation around axis \mathbf{v}/\|\mathbf {v}\| and of angles \|\mathbf {v}\|.$$ \end {columns} $R_1^T R_2$ matrix of the rotation around axis $\mathbf{v}/\|\mathbf {v}\|$ and of angles $\|\mathbf {v}\|$. \end{itemize} } \parbox{.5\linewidth}{ \centerline { \def\svgwidth {\linewidth} {\tiny \graphicspath{{./figures/}} \input {figures/relative-pose.pdf_tex} } } } } \end {frame} \end {frame}