From 76c9122bf19fa0fafe7bd7f3474147050bcd621e Mon Sep 17 00:00:00 2001 From: Florent Lamiraux <florent@laas.fr> Date: Mon, 30 Mar 2020 08:15:17 +0200 Subject: [PATCH] [LevelSetEdge] Write documentation - reference ICRA 2017 paper. --- doc/constraint-graph.png | Bin 0 -> 375821 bytes include/hpp/manipulation/graph/edge.hh | 98 ++++++++++++++++++++++++- src/graph/edge.cc | 17 ++++- 3 files changed, 111 insertions(+), 4 deletions(-) create mode 100644 doc/constraint-graph.png diff --git a/doc/constraint-graph.png b/doc/constraint-graph.png new file mode 100644 index 0000000000000000000000000000000000000000..c28a8da7e529fc2e9ccc490756027554127f1c53 GIT binary patch literal 375821 zcmeI53A|0!+s7YMl!VeGGm#{cN>S#JQlT=GkTegPqd_FiBbvO;f6X&ZD5*5ifHGvx zkV0ji?f3aEt=qZRz1JPiu+O>A^4ZS0XYalC+P}5;`mJYN_ngt8O{u-g?<FFo+O=)f zQKZmo`h9MRJ@o&ogO@&|AA9s}-=>uW{CjTv%a7?VdtTkP>$M^!_hTz0PmMfCe=L4o zyVF`1Us9xEsRp%{Tz%c&B2}batLB|=DkKeuOzvMO(4vP~F$4?&L%<L)1VV{G;ZU~9 z8f6F=0)~JgU<mw+fDx5F3Wk6oU<eojp+>-nD%9O$jWYxc0YktLFru<&zz{G541p*j z(687%&Al%`6sxtW{zbq@%N_+ozz{G541rK1U_=$_?y<%h0)~JgU<epd*)w1W7y^cX zArNW=jHp80J=Qowzz{G53;`o5dj<>vL!d_81>?;Zz@CGoK)^^VDZ0rTX$TkshCsnZ zz=*2g_PyP?Az%m?0!e{@5mi!jlQq&1Fa!*Nf{TC=Rl)6hyK_Up5HJLi0s$kcr06DV z<gN&`+~@Cd<_oYZF7}HdklF|sX{C1m+T9rfhJYcE^avPHC4Cp#4HyE3fFY3D2pCbN zcK_Pl83KlYA&~S47*QpC7upRN0)~JgklF|sQN^==H=bDJOY;SYC*pQ3L%<M7H3S+C zncUx)E7f|{?#B=?1Pp-`L%@hC#X8h(#}F_C41rWbz=$f<derX65HJJ`ffPf)h$_W8 z)NaQRFa!*NR6`&;qWZt0=Y3<o0IAmFc0Y!IAz%n3F#_3<RubQqHQf*}1PlQ~AXN}B zqDqyXwEHjw3;{zRMG-KfO3{wB+cE?U0Ye~F5HO-jm7cWwFa!*NWI^E0PoEiSz5sdb zEhDWw0%xZg0)~JgU<kwq0VAsT;A~eh1PlQ~z!1nI0!CDM1kO$~1PlQ~zz~QJ0!CEv z!P%~22p9r}fFY1a1dOQi2%Mc}2ox*?Zk+i|H}eH3SbZMOyEW1ZXTMmh3;{#H5HJM( zMZk#49tA_d5HJJ`flwo0L>21pvBnt!hJYbp2pCb>Gmx|h?AWnGf*?rR_Gi{`?kT&% z{chjBJ-b6}pCOQP2oz1ZJF*)}X#}=z-70I>u9Y=w*2wDBt7XqU_muti+fND?E-YKN zY>~b9-djqREV&bdc~!40m1MpE5Yfhs8)fCnmG1uP)~%amfByda?=pM#Y&qeC6Qp?Y z;#nSNI}CwTLcmBXm3lM9?*)<?GiHoD`|Pu_dGlsz(4c`Ms|5=dxWBh;+a?b__@Eqd z#1Tn_#D)zU<b@YrkU@h6$uY+qBk#TUUZ(Ab%wB%^Wp|ISzy7)-t4t5f!;g?<p+bf7 zaHgGN2*d$_hC?RzH@1oc&~}OZBk=d%f6G1h+#`=a{<!<R_~MJxDn;zN|Ni^kKE$kK zD~N=ro_eah^UgcjHXU*Dj5E%V3Kc5IQAZt>?XfxC30^B!tdLTrO36O^?32^e>^MUp zt_T=W#T9P5SY8ldFh!iY@4oxwy6di!NRpzyV#SKN-w><Tty`Dw5*T=j6e%K*8K3A3 zDICjIv}jScMV^F+!8JLS{gL7ZMkW5{7|y3owwxA!UCyE1zECgM@`dBLM&vQ<t6sgj z+t?RbIG;9Wtk)lt)&9woCp(euh8u1$qVizbXXp_4@68BZ)2u;;z)l1(;0zx=Tn;$k z0O{Skcc!qEDN{x+y67Tj%;EryTFaI#6C|;U6)Vcbi4*1U!w;AG_3I19o_X`;xqV0} z{8p}9Iae=(J^IAoe*4Xl#DNDM=!gMwTE2X_EM2-(_TPVh`RAX1Wa`wZa>yZvI6WK^ zNp=OIKysNsf4-|{uf6t?+O=!D^F7gV4TzQFzWCw`S-*b0G-}kyUEdq!z=HPE-pZ9L zyGugO)QbVDM2QkkFG{;85E@24?rGt|g>L_0haKk5<?ja`ct9X4BsB^|XvGi+8v^DA z7@i&(F=Maalgt+&{^0%j=b!S;H{ZxX2OT7}YSl{D00w1@ISkx*Suj{L=s)qq6Vjwf z6S?{3n;qd@fBp3`Y}hb2;P>g%#|_kn9(t&Z8Z}BTyzoL-Cv|=O_1ALKO*cu68Z{hg zTyxDe!u6+5pDsOm^pKV<Te|vgzx{T1jgB2V3X(}iX(wGH^^c!^`bn<6_F9=XZJJc8 zR!u(r^i%23p@WPYH!kf^h-uNHMRMD1w>ecO$DeY_Df0Q}pSxpz_~8e+;DQUJZQHi4 z9)6^4&p-dXeDJ{s?wF^ZdP>eb^Gru#`~Y!3_Sj?c{`>FC!3Q7gj%(AVjZB#`CC$)> z<T-clTt~>R68p~(2pa<Wf>|*HLW_V(CBgXd<I#BqEn2kLrJd@T5a=Kr3>YvVxa_ja zf;;ZGBhUdoP)R6IX(~AW_~QeWv;rN_1C>4k9ngbHl_~`@XU=qWDNzMVfPoT3pi)Sn z9u7gLPMzF7CBQ(XvOouM_qTdL1nPPh@JZp?S6+E#P`Y&KK-HA)x6J-qvSdl1Bo`<_ zrtMa#FX-8`XP|`W+Qu=hTel8W(hF3Aa>q@YG$~MW2~Irm#6Zc!{r1KiZv-l3y8GaV zS6+D~P|3*cqrG6EgdHf!x!)dn<dL9j*RJk6|0zKQYU>J=wB2um2M-Qv)~p$<TD2<e z82e%fgbe|cuEI8Ru=ebNfVUT+O3PSK<27&I+(}cY3Xu+QIzXML1me_MsOV7L>3}Rc z=!*{a&N)HJ!KnkO6Bk2NbLGpIm$75VI;rNbzy6YAk3CkD*qnL}bsbI+-k+RnpL_1P zu1TML_F2ZJrTvXcQOVY+^!$&gEs>~b!|2hYWsg1faB4@cg)cq@wPm+%-DJ+3IcX9U zZNRRD^hNuS$dH<paOAGL?n=83>OfSTNN!D=HuaZvD$g&!{8BEx^in4=qIT3l$>ElE zkbN-(LWY3Zt3uWx)|~VRKvIwl1M?q${NV_QK|Qh%U{tkgRX5OAsZz!5V(?|lSQx+| zu9seV$pt1sHe3S|<Qk~WzW@Gvx%b|Cok!k~Aw#5n`}XPTiX=?D<{%erLRl5|p`wFC zBMbI7$PuF9+LXvPLyFjzAYJTlk^5;g_AG2yty;B`TW`J9?T`EqH6}JG?%5ZJj%!*m z1d<5>lc<uZw+d)mac*eWuALjuvyxmGxEbUlNhBj&?89LJDTv^!UcGubg5!72r(owo zGE<eB`x~3kTW`IU&dbjq%K@B=goE8Fs{$Ev40fQ%LVMYV6qi}MxsE3m+|x4__CaFU z&ms$TVYh=AH*MOq%YLLNez#%>BntxhB&r*peE3@P1xS|8PK9>vyYIfzuU|i>D#LXh z^;e`6!aXv0^P49WJ~NUSs=n7=d(Ams9CzGtZs5kNk3k+calRAdgd~G^9Io=H;`sE1 zI=PlFsPXt42Z*c+gwwELLqYxK&!rwDBP1xCHGKOc$%}R%9ifWE(Ic{O9BNLaukXJ5 z&b85#)QlN3oT?Kk66X=3i+}v_M|ZzSdN_G}^2sL-Plyi^gXAcDw_*r{4S{@;R@iRG z+LMY1Fqk%L)=Zv!@<~~|c(L^D+gI?0!;KvE8ZOyL7*$jSh?g6ML7u@kGTw@MAUy`_ z5hF%8;=r?zg6lS<L!G3AfVdzJ2#t*b`JsA4O-Q}ik+27ye){RzrpiD~NL(1#B|Qa_ z&9A@y>aL0RA^R!VuxK+LjF2k#gq!@pfdk$C$p0WJNba0-&T$f$I+X}2OYUpr$dS&i z9lIUUA+GKmH+1Mw7o*1g;T=ir79Nw_(}y2^=xk!@U=ev4`)&w?0)Zm>5)2GQORN!w zKsE?q&mkVEbLY;EoEY$t3UK4bE`uuV&O7fE_4sp=5w6=14NeM38ju(&KGb!P9mK>S z-Lq#;M>t4nkRe2YM1pPX=bwLe?(q;KlEyE;{37qZ`>tyT*M>-Nvbg%{tFvjjI9n7> z9S{eEfSWjNKvFvY{PW$pv<>M9qC|qDZpfCPXU_Fy)~s31-5hd6T0&amo^jsbo*+|V z(7ZDRk{H(|{)+qI{*g|-^9E8G_A;a_>f=}|hCtX5D5NTgU9HbyYo)cv5J)frNLZ*1 z)iqtPnRze3csC2%8N`S^jjXj+41t1xfT{ipLND7rMg;++Dz<n#C15*4nnT@)#D_G5 zbZI4(5vcf4+y3SYkXa8KX=R3r{bmScKmeN#b|1X=upvb{Kx8;Hs(+&VK-Gym{fjTY z=xkoyyLXqeWy?nOI#!t>5M~5S4H)L$v9^T*0aSoUHw4{695^&!YdZSqqvO&(q$})P zIDTN8!YRUvAyAMIFrq3*y>0hu2p9r}KoTNgs=p-cG;6XUU<eoj1qlHos)E$pcCUs& zED<<juR|X+Uw~L5Y;`LFMq2h57y^cXAz%oE8UagO8|v<{#)T09oGS=2#^HhuS9U(} z3e30>m>ao~HN$UYjPRM&`OS(UU<hP^K*J%E`x{$j0gnA_2*5&{H*a=9RRlNbLRHRR z9KUtaM3CDF7d<wg&xzyxIrz9!$Xv??Gm)MGH$sy1k!GE=6D(UyUMNyXu#dFp_{ICI zx7HJeKtV^q+yD#u6JYnBh5!Tuxv-IMi3EeBM(30KMEHyIJA}oW8W0PF!^U?=h~FU? zzXk_nhX_WFC&-Qje<TDXvR7AGboKE&q8h<ckQce2$hAZ&c=98$9tw4{Wn2p-t?~B7 z5J+JJjHpt$bM4l$MPO*@W-Zfv0U!zpgpG``WThoxpDr2f(wjq05Jjz8wOno`(neIS zT-ixPx|Fb!gz}@qgUBE*qKiqV&gzI{#3i#W-)R>Fhy+HmKC<ZY$yzBao<-ZN7y<<W z0VAz~(93p@X$W|7BAqyrlh1xjR#*y>O`}GQT*5tFOWtLB#po38g2Y7xGFfF=?}Lr? zL&)OG8YN_>Wr<_5(UN<~NGskwWY>!)0!CEvMBA>FFa%J$VP`^GVGR!m3aJI@<d|cQ zkrpjlxHRiXFJUc^DVcM}k00-(KkQ_v2~R!sRB6+ujUz2oiFj>TF$7Wx0VAqZ>P@?s zbO>N8f}9pDTIALa`RJpMoD@~ATsgNUJ!@(pJ^8ic)14ghKadC^Ez+(}oH)_BACmLw zth3H?`Ik(}3V8=w^KylN5mm0>vV)TW0URVylVNjW-3*rC#j_5nscYA+PI5|wIxkk( zvB(xn7_)LBi&L>67a49@p16AT>dxsSR@b(A4S{$dU_=!Ul%sZym&zP;d73Xk)M`z| zideG)NeVR|WW@kpr%oN|)~%bIefHUIN#Inx@5p;%nd4`kc}B*L9qS}6$crU`@h-H~ z?2$;>_hdrANGq9o%i5YL0!UMM(0%;z$8L~jX<bs0Uv$w$(z<o)OwUR6A6e@6|Nig) z<bVJBpL4@+-@d){>eb6RW#DFS#Slm)1dOPXskfrsR=nYoSsjKw3WCB8HFD%gr}{hj z<dfx!E3R<P5>bXLDXSwM?Ad3Zbp(!c36kY`=bh))s>cmJ?1g94rw1V=2z}1R?>_wp z_C~%FY@MIbfv{syklSZOm7={*@G0)jsKfa5-+eX(J{bTAEG=HVxC>nMpA29=n3cfR zFuwfQqp(8_A3of<bhmBW)~zj%H=mVV5x@~+*sx*p!V53B0ljbEzK*o8ZH2|Lgy)gW zAAb2`zLOOMdmP@5#JCZp&9e+y5p2!#=`(=fFhi`{zWeT*?)PLM<XwBreu+fDh$<3| zB>oOD@nVw^fk6rT%<S2-W%1(0f*OdIofn;%Xk@ZFNC>IQ&q|5SDPO+4+lVhB;FXPx z3<R8_HcBLMaSuQK_@f(ivBNZM*idf1`DPbml*HW^x29vmyZ`?Cof8KA+OJ<f=V(IT zCZh1Xvf-(TEB?3Ne(OF_-x3(jbImi$go@`5H+%?+?+_OIAuTU;v7cv{%pX;&R&}dj z5*tSd0{2*^Oc`_KPee!O^O~_IsxPiR{(JKU$Y(e0{&ZecpPO#lv}rEe0%XO&1Q9_n zxG(ea;uXe=%uJM*nhj|R5@0s#H?k)XcSOEB1`TGj3>b$VdZ-+E<dKdfNdE(|#nVvF zz(u;FH{N(dMvNHY2A^xMz1FF;c8`=Dkr4sn%LWe~EHh@zkWQUC$yHZf<>IsAA!$K2 zNKKH`gb5Q|kTYqlARzK065P!5%Eou{CGs45sXy{wcn5e!$?n1A%%{X6$eZVwNgEml zq%O$oq?1l^OFDBM2+T@yAP{@fN)8;7s+G(}Arz#HF=NKKpjKXTUTjkDkja2`&QbB? zCx0B|#Q^ff7hlL%UwtLhr%!i-0qPd)LhahMbCMT>KrB@)3`D5>kP0AVT+eU0<rbkG zR`QMjs^BM{ctRd|<PnD@n6_-$(oJUaUTfB8GVy^x=*!WgN6XZyQ=P<f^wCE<39otc z=FagW>l$*uhlw2VXT+j?{q@(fQ0<!pfAjpZ)va6Ckr=ivE8#`Jh$_5&vrEhTi%#$- z1chp^Ql&~RV*x>}%ot-?n3+OYZ@>MvbA^U5Yu2pkVtL4#fRu&A7R`cWhuitX4?paP zc+j9h!hmKadI*rd2~{c+ksdvIIO&SP+!rQ6Ocszp^5Teta|dan-hA^-dH?<QT?Tq4 zRJhSI*+IIAN8vu0bn*TW&cO4_J9F;2=eo%q6E4g45s#k8!!=Z;VkP7VsQoF>S!i(e z)mH~<LkcvVPM}_HfvQacwLK-I^=canH1IUgc%48cw?Jp6!O)>Y10C!F9n|vOCROPK zIs*=LFbh<23RKM(sDzjAdaZ)Y5dhme@4VCXWrq$O0wu4s166VlRN4wujh6OXPQLJr zs3a4p6Gx!xyFl$@t{zQqm6OwBdW@1(pl<wuN>zc9a-fr+Ks_Y`&7vRE>ssY`Lja?t zm5?H!5=@|3zJn{TyfRS3aG=?}gZ};d2Wo=~Rf{K2o*ZbrjvI_Lk!o=F-FF9SAILL_ zs=Xn2_0?D1zy<;8OgvQgo63z*m0X|!(}7AF!3Q6F;QCOrVFYT^44!`a=^Wj<W{VG0 zMe9gOCH{bCV#tsofqHG`==^vd#6<7ici#<EvU7x`nt?z=9|DLh^krLzLppk2fY3KK zdJQzviXN6xtx5Ij!Q+oV9;livP~)<D>D3q=)oVn%GF9~imt1m5pz4&MapT5rFhDZQ zO<`bz^wf~<Br8<|=H_%eHVO!+7ipk@!|u65;!rhgpbi(o`RAX%+uG?r4L}YWG-%)^ zF-)>_nU&q17ls2=Js4<yNH;-LN0UIElmZCYO5zbPqDp)pq-%j1y93p!2h8$xfju`k zgh6eRu93U>1NCI9R;>c{I1Dbn_~L+pFDHfjL}jF&g~3fX-4v*OAt$HVF)=`ZiIUo` zoC;Q*bON<71Zran_&u|tuHykxwgwryb2Q-C{hisdVg5~RYfc@iUbF6gUwrY!0BPS! zVi7Q+N-X~AuCCrnPG`<bqFzx+t^R^!9(dpZH!IiR+(2WY0(DVNcfXN;Xp-4LokIe3 z&ImO7MC2~}9%lrQB<{QKzO(@wl44`x^w?vM?a~@-U9Y|NT0lRes($|Y=XW_K*?vHc ziLDg%`;9l=m}^^IvfXdk+ba!+Ozv-N6_RdJKi=S)Yp!u31yj>x(V6THz8`-0A<)$T z9Sf-|U%<3AvS2jOARni9!TOHDz)Io}z^g@r*PH<dLlt^I&sKcu0$J(2XVewd>890{ z&*=>Du1S^RJx;hc)EvyHtRw~jBdWyUp6;p)<_rKBcpiN4!F2ml;Scq*4=%s_ayKo% z@WKlNjns6vh0aK`%sT74uE!qWZe}GM2w))8Tuedl-n|2j{0`Jt-4PZpncmBSV{k`B zUyJC0+g~<i!f6+!tr3vTVd38O-h1yk?-SG>y3%F18=Io7Gtx?6$0Au^phAa&7sZDk zewe`4#QxHnlrCVVs%a7e3{>2&Mlc5&z3l#ueGBXgc|l;of(6dwg^7hG;R{YU;e<eQ zA~^#iUNEUR(aMMu2089uQ?>-SO)}xK5<dhi8d!B3i748|E+X;LOD}bKke+<<NteJZ z5p9X*HHiql{`%`(nhSFIkn@QY>EwX25^e-Y0YL^_`jA9?<VhMZV1V@L(<j^wk5cQ% zMMWMeBGfe?%`L$krK+u(-60S^PP8jUB0!WRUKF}itqVws+?VR#QJ-jHLFX<;Adi*g zL4askU9r;nqT^9f(2C|8b6!Arkq~@lB|Zq4M7296kHLOYNweA=iR@T&JTjL}3@#VS zDp|7jf(tGXQn{=B!{ufQWkan|X$Y_e3z7u+iAc)Wy?ghxeFf=@szL=#2+24}qG<WP zV&D6PV}A*|LL>qtMAI}BE|V-vwH8E@0uPO}l!qRA$gNw#iijjR3|~2|OQH2XFF^QO zm#7wz*zeItA9a%~vfLI_!UBI(*6Q2u*2}r!h8sj}S&8bF0&`s>tvGfy3-f3gzgxot z!eWh$f?QZzo)>=7q)9Sx;6P^sDad$)_1;Oxu2Hd03f9mHx$Lsb!rIihwTm@z)S<*J z2**2(4a-XG5GbMp#K729Z#8=aFhV0uXtF!^!Z&Ny%<n3&pQP+Q?6AXJYHMcetY1>E zUcCZ~M_6zClTSX8JMOr{rSsR^S7B{t+}p+TTDfv%88vE@QwcKhvUNpb(+x&cvB5Q6 zt?H8Q)<Mu16hT#>F-Gb37vw+i0@UoLZh<VMOb9$)>q8V|+;e*GsI70m{dTt=JS$tW zGN+aQ5WwEb`>iS49Yz{<^50JT7BvLSu_9_vW?d1>^XeK4ZuLVPDHxosq(^||i_bdi zEVn!{tM8RAT{`^%_E+2yz=p_j!8lSp^UO1%-l}nLLo!^DWsRADYDNpEEx{IRCAtXM zh#6fxcd1R&vp8=lcCbod%t@;K2VHJYa$f*ejnwrcWX_y9&VauQsP;qL5zx>IQP*_m z`DTQbHT=W~5_iSY*r>BvmDEaf5irt<E}rRXLp{LDjzo3aZMTW8IG1j}{qY|H*shv1 zY2sYjhYug_yafN-Yv1CH022lRv^9>%5mmgepKRA-nPW{!C4^&Ozp@ft1dOPni)Xsp zSYIB$6-^x^J$v>{x6l6A6#-_X#9XnCIJjwu;jRbSFEK{|i9mx~W$M(aa@}>;nT;xk zh@W=aY0|J^L&5HfTH8wW5HO;O9+v59(v{hqB&C_tWze8O>Gs(lStEedi90lss3r-` z`as(gGXykCx^qp}rE-PMO3o0#BajIxDX5S}twaw2BdX|OnXV?JDP818x^(Fx#~yoZ zx_$OX)(8|UR!npmS~oK#8-SJgBCvk_deJ1pf}K^f;>Y*mDR4E^*C(HRvWyusMrO{O znF4ndj#~%?QJplj>$)WO1<0~DbqydV84&o}uV23`kFy<_BY-C$uKlQ?bUmxg53*ll zf&c@pu3sg(th3awUq2?-NaafR*=HXYhX(l*OKBxK2!w*PqH`;;s|C9Wq@#-;=IA1* z0Um$+aY27gpsSVKAwV!CHa>!aO~T3@cKJD)VD(Qw{nVwrGW%-Yk<_)V-1EyMOB3DZ zy&^j^4S}L*-;($XM6zMS2DdZ;pSpIsAl0y30iSr6G5AGp#fukruB90BQ9Wf>usyx> z(n~IV3Tad@{AX3T9xpp~s%M{lHmk$z_uU|{@4ovw-9IkCcvV@<%5DIQ!fq0>;!x4J zabwYSsiIJ0!K{LW)VhMOOqeji#irq$VkLhFBqdRiIFsNzBp0$iBkl0_KmYtAt5&UY zgau*v4WfdK{I<s)dpNHqUFFneIp@LRJ7T-8Zko37H}Oy$520ayf(Rk2YSpS`iahz! zh+Zdh9d95jc|(A{LaHiXzP!WJh$?S5=H{$v)26w}7(uO8@`nJf>vikab!0_iW+ujY zDR^V9UAxw8j6Hrs`em$%oRkw9Q=~`{H_kCBXWY`oFx^<k_*JrG$+U59ca!tHv^A#Z zG#oOye^Rg&y|Q@mVwY?Sapk9<eiEjD=;KzeUhVvvJ=5sYq>h0~lqey7^I{i0P482m zuAwgEj^TX%rf2D4dKa+?ceENcYDk3&6<od_uaCkPmFtpTI%6&su0uP&{PIh==bn3F z+Ia<`a{97u+qNz@6>leRoR!!hz%xBT#Q>sNE!sE#7?U6n@f3GJGBWRO%@D>V(u6P* z_n*wdnT@mY`<gXt99uD7q3ZFoanwgJ72^-HYsNAC96NH@>@;5heuJT4O~$xVrAoPR zj{HzW-!m&DuNlieurJId)TG6WG<)`J(KX&35wK_s9R;wu-n+DE(?;slspG^Q#H8FN zEMs~CX>lD~_)su&JqVG7rpAsP>mCBcFG$P}n)p;ho+L}-VcdWJ{aqi^He7*tKu$mX z^f32(QnZbiha6SUJ@=d=uCvcRJ1N?ojEy8sID>1;mMxvCAsO!>Y;8uxLV9|}kx0^l zU{D1y?w}fCTw~0cH*cQ%?kCcyhL}vF7iS_3!7%nQvA_d~@eTsw7&ZtCGK19E@bEwl z#n{HRAU~EiW;|u=;~WSJQ4j+$26omu31gnKP$mVW6M1RgfB$_4D&XUibka#DNwa3n z<j5nBjQKMO=|EgOK)?R_tIIQk)ReLG?b}z*J@;Ivw_^4Kc`{RBjc+UYM}U{BQ>RWY zo%PEvznuRHt%h9?U{?OZ3okewH^$k>L3Njd?S~vA;FAR|T)5ClS!f9O&Lo#Hg~<UD z6olfX9c&Ct!r53a2<;9N-)I&zOAr=G41fIbN7oi4I!F-Tdo)StG9j_Bl;|Rm&$~jL z{pzc)oS*00Z@=vZd?b^O9XrN7P>F|whlN=T0Vhao6qLe#CWBZFnKA>Sa~V`o{9ABJ zJ|BdL)4>|G6eNYJ1l6vU=pw+x5Dfx@A_J9`=pcaPbKG&qId3cSt1&)6UKFrK8$`zD z+O=y-_3G8r#+c|_J?mPSNV5@>K!Gd~43PZBk00;i6>w!_g2=cQhP-g0Au9B-B+O&Q zJw(zzefr2n7hU8QGs+tB*zRGbiBxsdO*c8UAEe0x#>PX4jSNCVG>2q=_~8eqH#Qfl z==BS#4Wg-s4I3t;Q^;r|jNX+BZcV6%u-!6nCY{wi<lTmZ1g1z(XoGl`ApwXfECsb9 z!T=j;NL)e5RDAQzH@o+(SFc`9%H~~6R0#!9G0V93-h17e1NeDffBp4NZ<;7j{B_Y0 zlQV@SKhfW2w{$PFeRRL*c-gE(2LU8lvZkUppP+_+BdX}&8d(c7eFmt+uR}ke(cZn& zd;ubxm=fQa;6hM1Y~6qV{h_}n#y*yP?$)iFBQ9*kS6_X#Gj$+Hus;!{a3U@mc})y{ zER@3{IoDo$tpE&$zOWSZ$VByKM_9BK`xdH{<BvZ+EKN_DcHr28y>jBji79hWVYv~s z0@zQm30tg1^m_%Of~2qyp>9o<LJSYuEmCisa>^;r-bPFV&q^XnLWv5A1gQ!ynY-?~ zO9l)W;JjE80YrYUiA{?&6ifw}UnF-uAN2!E2ct@{5?us%ZW(+~X@jwq=puktfJrW@ zR-7u5t>9ci>KHO`fA`&YF75uTS+nAP8wn#SY(dzkh_EGA>Eeqoj{D6=>w?@1aaFj7 zT0W&{q55Av*e_A7V$)2fS?Paw5Z-TipP>T8W|2szl^r{_ORBE^f@d8j1w8(ewP3SC zs6a9#8v^59JWImtRk(oTuz;6IvgaT1>Jx(O@EEibT?8;p6Ih9QmDxW5!d9Y#04WiO zxydRGOLVT1c`d>At@7)P=`!!P<+7(r&Bclpl^xr+Ns;1v$-xz>NVN(FWPW0j{YuOw zlYnGj%<9ZD&vah62o~7e(1MW)BOWCoM8&{P<_A=N+-$NJg85MUVM}3Vn%jazwsy5f zq_5v3#flbjl3Ag`MWsyH{iIaMovzuNH?ETvEB}(9=$=yX;GMz7xxG%Z9#2ps9+#*9 zAu8N|LsQnyzG8Ta_yQ0Uh{Wn$nB#@<mW^xW(+}U3w?>bbealysBMz@AMT4y}{fBR5 z=Az}&w)+**H$_MbZ(#=acuOR)=*x4?ImeBABs|5wdER;F#o{u%)R_>{LY;#nMUNgm z+^l(*RwdaFkQNUi4<z1exh_gtt5^Lgqu+T;rq5q4jZSPT#e&VUVf_XvSD}WqYJQ^p zxoD0&ub367(pYZ2v`elp5rz{`^AyTsL$If$%FCWd2x4*?(iQ2);u=E}Y?Ddj-jl(1 z-7hDdbEOQuzIWQawQbc{gOXp89h*0${g%}iCVQyYGM4;2XBq2?#@{@<IB2DU3Y!Te zi4TZ_#kHLUyHIn+y2PERc({m?M)eRcHjXujPCC*T9_cDCrI2<$YLBfe=E&nCUXzY} zuayI~|0!cfzajVB`=DHR*Kldu;9!~k<0RR>tx8@lk<fEc?aZAqP0H78AbaPrCB(6z zN<jj!5<LXaaG+kz>$97emaJN~P+lALlB_9qu=Km)Ka!%ZG6z<Zo_#NuMN3taux+#a zJoQIey+P9e?YWmM)-~blG-xCT?p;_GEM6`})Y`sf+YUMGh<Z}0L{a(suNCs!;$>1) z-zWW9Dpjj48~<9SYR2ECNU`Fw{;%b-{I7LV`^W~;@W}YB)5CL1S~fCPq(VVuOaSu( z4?N%khjD9<bx8<O;Yos8hq#$kC=e9}>fGiaOn2`(Gptytvh2A}Svj<J6{);O6{%CV zj*Nfz6&XI_8R<R#d8uBxqRi**{I_h|yg?Q({av;Pg``r|YO=?FyJXcL%VfphYot_} za&pK)JF{(Y+TSaFmt}waCHw4mz)p3dwtb7vUjP1QjqG<oIaxA)u52%|msGD>Q8uk# zEejSdk-hgjP%1{$v3~U`S+r!ilqgk3s#V$<W3g51Sgm!G+PAFyzG%K|FSfU~_Ym2! zZHr9#a-2N;&<MHg!RMsjK_zAHlDSVn%S?`!9gj6D(L=x|K=t`8qgQj*H7!#|j_-e( zE;XB+D+lb<a=U{!Z`vTgFI^(*H*Hh(cS+}n@Zp;;%G;lPFYP*ZmF4qi$=@4QURw35 zEZ9&=TGTs4-WxMk8lTx$&Te+7d_L|&neqD;X?=86`RV6hWmEC8vVPVi8T#<ka^PXd z%25^fmQO$bQl!)vxnt<P(y(TH;)AfavZd0qi`tJYK7{9_EKx04vc%nFyo_>LzcSI; z15xo1lA$<1WycOxWoQLDCcCnF<)5-{(+(+rczGT0bs?@mr-}cTzyDk;ufG0{99pNo z{IzthjQwh=T+-uA3DlEn_O$P0_L9G(NU&X||MardZ+@n<Yf@j9%>6}XELbLMbmlqh z*LiYc`?KYwx)tTiaqr99AAKWTF6b`5PX0!w{W4dYw(ltW72T+L!hVp&E7r;tHw=&} z<@S<QzyBsvrp=a(>;6$)^<1fbbaOeg<uNkli%;a0x5mlIojc01U%r=VbC$?CeXfw> zYnGGmzxY_*|9pyc{B40$+Piw5hzgY|_9`X>R-%W1Ur!>Q?;=v%nzifHJ-&3h8`+_D z?CDd!l2Na}D^-qaF4te$L*2Zq%i1xMq*=Sp(xleDGUJyS^6l44r0MakrFQ8J^7^_p z^4Y|1<m}_i%gm`i$gB<J<l-J3rAF-|WL>bgtgld6O6^fdDja&G#%!K1r?eg`S6nkd zo*DIyG#q$Yy6ea44?I|Lx<d7b^cwA)ez9tIcJ&g_j1w6deOQMn))INd96Y;bMRy;L z6?9&z6o?A3K3@{mzhG?Lx=Fs7_?=ArY`nbw>MOEG`6J}2%X>?qZClg9wS2)0dH#)$ zrCGBRrEZM_WYA6h<u5&C)v|f&{{NKhSGkU~Z`VeOu3IW^eKb)vum4L%JamsNShZQE zUUb&&-WQ;<N<=@7dtEN+cdM-1QAn1|nJ!O{d`_0E+9XHRZz!9VPnX+<JS;m(l$J)v zA1gmkcw1ii_*>b&X}!Gm>@fLe<`Su*-g&>zoFsiOx=a@RwNWLiS@PtQPs#j0R!i-A zN6Fd+-^=|^zAQyb>?3vSH;}TbuRlr`_9|a4{XmuO2DAACy9DYKY%5lxhXAC-vkbQJ zDkVx3m%aDeQ`YEU7g_c!SxTy_ifQ?RS+aV&W)|0ARHW*%Wy{DRRSuED#rD+a^JwXC zW@jlaYh=cp`Lb@qMon#|FG0xzHQ8!CnLp`Wx$63X^26+<Qn6e~odgz=(ppcs{dam6 zR;gA~8rH8X^XJTqymU;zdlk0Nejd|{#i?@aZ{!rj_rT{kjOC@{PFf5&xE*8A#UUY8 z3NKWC??<80yn<U~&M(tt`m~?rpRL7Z$U|zYI_@ypx@~(ps1B-9TW-AO5}BvLvA?fa zFU2;mkw4XzGvm9@<;OouNRzr%q)?G!a?uTg<?^m=RkE5cZ;YENRrcRo{#dzMPCm1% zG&<~H*{0rwjZSDR<@PNjC$(xK#~gi>G;MN%Y~EH(nx4=|GetC%21gvKLAbMZU|BBz zd-)wXtVR|2{f`yW;KVj^TGIxyacdzZp%dhg1IkLP_NPme6ONY_Cmt({ep~3;yqBu) zim3WhbI|6^@g(BTGc%+|?6$ADM8M;j-)A>Bmp!<A1!>seNSQVLdzr73sK|m8UAAm# zozxaf`#Dgpf*&gWSUg8wdEpuPcE)0<QB#+$sCcky6mX>wIq~E(<eoeG%i6`$WzfJu zGIsJDm1>GPA`bqI-r^nGd!JJ31XVVz{#buuBEUxRc*pvh@u;6kI^-LV$z$B@RRBn2 z{P5w!-C~EN9!d7X!$+s(*XYl)TwEoPb1%9;D(y!2EK+n2*}g&TNxv<TP3tyFk)leJ zDz*N$V4k|R|J$vF3aee}V5z6}rYWC%Cz}eDmgX&+Nl`tnaFGsjkpjPI!+P1G4;zJ# z?Hcp4L!~T;W#@1Dx7x!3RVi-X^tb%^*LG>tw1reIsezk?i^#csG_BO;jWTB`GjsjE zb%$)%ingQDEWGpOWDw-r&c9r>x&Dtj;_j)I>sBV;2}n{*vf`;;EvHUI-OukWGw*yr z9(eE(x%sL-QsIDo9e8V2>tcidtdr_RcM{b$)hcY-v{|34o!CNdufFoK^t|Ia*|y;y zS+sbmY(3~e`D58{GFPRfIxWwXXKNiQH{CK=KK|@$sdIWA*}7%3hJ~zl6)alxn}&yM zmUeAh#;vpbV2&y<o_4`l-yrWJ6N`A<Q$i#vjB@C0KmYvmSYsKN`XMUx{rNGR7S=(( zhz=Nx_L&Xnh4q0btk>a_A^(%No|VBP-jFt}TF6mHRC5GTLM4|2)Mhq!%BM1Q!OkTI zwr*M{KmIUHN|xPUHvc(W#(n#1S{=Xq^qs6+yG3KVb`o0={V1g5R=BWEJDGZF!8?(^ z3#)WiLREn!3a^)UKm1gUYDJuFZNY2}<ox|_R|jW6aNxl?)a8oa(+-^#=jUOAsG_B6 z-PtJn9|SP?qne5*QHd0l#;wng+i$)`w*EOsUVq~q89VM%P5eAjX3bwB^%}R9Rwo># zGpF^kaZ4Z-D^!vdOBT8gJLr(ga%i<G^6mKd)F!%E_CN3dDZF{LdigDt<x3aJr=NW* zD^_omX06*vo92yl5kXx!cgrUEZT@W4Qv4*-W-XNJb&r!X+nnI)i^qScSIK6>q##u! zD#khz9+N9D9)bBgN2qx8*1y-FqsWOxQrD-Sep*=Bm6s)%izlkQ#!R3VSoOyenfmh# z*{D*|_mgKxxdt_)<evYERl0x0@+C5V(NbBt`8U-I|E(|OB3Zd&xqL8oylh?do4V|- zlP}f4-0YYZa^%5JNx!T5N!K%4%bvyekbMrQB+VPvlXi`3$}QJiDN8QsCMAm%R`uQv zIi*r9nK$QGS-XCnES&$h>{E7=%$vVJ)~s4Cf2>(A+y44f7V4~ZO_3$CqgYw#ct&%1 zY|xDoth!kCD^=V{PmNlgD!(n9Bmb;jFAL@^kV8})oUOCle^%^}O`EsK-eq*Qy!jt_ z_nj}K)zOElx+eF*(hwB}`DoY#llfVTcH6NVLsaol-$b^(P|@Pj{M0UTOp_M!^;h4> z@5`3T9y*X$taX&MKf9OgTdIVtRtKP3^&3mSx{akw(M_si-XXQ>Hk1K_hRV-MQb(vQ z^!VeBmFcq=N~Ow&s_J<^S+abEtXx+_8XVtTYSpMLGp9_HJ#`YfPw9Q+uN8kv#hORT zX==+V!F(@H1)@T#LZV8Qf<pzX7PAPWU(YY5_ttR8<o<DIEAEK5gH;H-bm`)j%;hud z(!7Wc;8I1}wQAlu>!ZT52j*g$LGW0fiZautjT@v)rNiZl%gf8=6{_FcQYDSK5Kr(g zKk|f=B`8x#Hf-4;Z9ASPKQA6B-%Xh=$2B-YQzCbll|&?;+*Iy=;652X@_+K#go$#j zjtpn1F{-%ikXwh`Cl5dNgnTxBoYbscSI+Boq3mB$MWH>*$_0IT$@bNM$~IN#A5^`r zbT4_RT1{6;8P(?<)mX2)Up3iLw1ixC!yU4v=)*GU^YK!(#$nQ_+xb$fLK*peT1n~A z_d+SU=^xp;;{Z9NRs%Vw;-0c{%?7DZv!3+r)m@g%{9e}7uAC=`U@b=1cueQ=)W9S? ziD^IYCRXx9l)cLwAg$YX$n?^s_uXHPIX+{$mcmlw@cM4^Cmmd=`oCYxNoDn(h%6Yq zHgDN3hg3aGnzlSSa#!5G^X#G?KpaVxGG)pXVJYFbt<|H``-BvgjRRf1Fwk82fv)tH zkTyi?qB?~NbfK$2lYa#|m`1DGnANi*Dm6!^mYg=_>#x5KG%zoC@4feOdWs#F0|a!1 z&_H$bfi7E{gQJt|kR98DugAX^^ytzlxO3Qx!TODx6LJGQyNw$+4s`Xxgj_$`7uDp3 zfvyn}v~S-&&?SYVU45qY<;RYYugL*Ah=BtKI?Haqe*Ka<Nfy>=7N<e+PXJli%T9At zep?xxT5{3~VjWmx8^wQ^mQ&41+rxegxqpbvN3WA=1uaCG0}hd{JuZ+Y^{UHXtJfyv z7O@c^QIUZ;*$d7FBmtZ~d$x-Ljz?}KFiRLwadB47BV8|RXS2E@H=m3JqGAB06G<O$ zCAtWp^aoemjZ)!dk1jg-s|_O{8JCki1$bsC>``Jb&Arq>+O}(_F&M{7g##0lo}X1@ znPgR|QYEuyCe3du>d2^3qvYk6Uv{g`l6E~_C80#cO|s1HJ@?$>>^rQLoQ$r4*r<5f z3Elqbr=Q~0cLnA;yyT>@2jh6AS11SoLsYDy$6!kO0V~l*VA--|&aXSJS%GtO8{eFC zcKO`<0-)i5uvo~dPMtb()m2x;#b@2$Qo>1#hIH@V-347CDqni(rEVF@WLuexm;dOa zk5>QNFFdyP86^bplbt?&x|7DERHaq(F9J*gaM9)6n>KBl;Syy8m<%xCA`e@#Y6fti zc=XXn<<2|rbXp|VbBed5m=JHk%$^1E$}`J*_~D10YH-4Y32r6EX3d&O*dvB{$x&0G zyB#}rtn;ysXTF-u@JyxONXd_i_@ax5U$zo$1gci8>Jl!JiS(qCPKtK*1-d>a-1vFp z<DHpi0Mh|dEzdY(2$}?fP{TlK#EVRxJlVw<jT<*knm2FmmIP*E6YmllqT<F``~C69 zAD8Ezdrn??<rPt{B&k=go?HD5X{B!6x|w!%yg^&7A@D3z#Y4!$HEY%^XPCw%mD$J4 znKRvk8Si$b;`Ojq{rvOK&K5{uo|Wh$zyymm9vGCYL>B?<fH+n(XwX0-lefr6AARJ; z4IkL%NeagnBrj6>KvIb$D(q6cbE8L(mbcz|%T2rqGrHh{3lipTA1b2a5yAG;t5+{k zH%JE(U_oA_*k(Cc2#c4BEgq5<#Dn-wh&CGp#q6F33&LX7QMz<#=K*-c5l1-ELaJg0 zWhMUz5J&apmtQ(ngL(huAJ5EcAexg2A@3T`dn(2LWOl#t{Owmw)QRI2;{n)esD)b+ zJ+n5nXwgEhx#k)tQDwG2*1s{%q5fs{L?kGrye3VWNblagMP2P<efezb4;@kY*5W>U z@x>QAcS!6$pMLtOu;A7B@#9_U0*sWkYu9$_LiE2F-Fe}#6|v<cmkFWdsO|hl7TuLA zS2|)MT`<J7R2SKy0||~qEa#!q*eQ2y7T#h8#CFIbx=53f7LFnByz`FSaKjD$R_rq$ z2rwI^|J$~0n~z%T)a($zU7a9KjODy%ojP^O?(oF!8+*&9db#%nNNk(+>PS{dtW37h z93fF%a>*rbtYW-~^uR<#$bOA?bD=McZ+?tpx$#(}`60}Pa1f+A$cjV;cxF=&4z9cI zI%(6UO(;@tgqE}%TKXR^6p|Jj5)EJ?qRyBx!@Xb-4-y$J%}8KKI&>_b#Bh;a(Qm#^ zyjkcZ4EAi06@-MC&UZ)*Ju<mbAVchE%rd-G$AiZ;G8V+=bIv)(Nr!a&kw+ex@eunv z7YNV?M8@+nk(0|x>=0n$#YFJkci#;+X~hnLsMm}$5GH~PFT5~~XdnQ_1;&#RBSy$) zpMB=8iw%g0`WtV&;l?7y9wy&RkkKNsv7d1*a${W%Kh8mPjCB;oI5yrP#x_6BG5N+j z(VH+9E?np)=NwD_<BHz8b!$1{gcIBZ-BlKX|3Xhxkr+V`Y@IuIcBFzz4to)XOFsD< z)tnC+<y<5l3MAqQ%XcP{5>gnQjm{qO;bCB7HbckpfMjM6b(LoLk9wP_BM1){GgTw1 z3;^ZwPx|uhx8HWNQC=J?u|t4Gh%lVL{`%`qqky)+O5PD*(u<=CB*MTSS&)tx!;rFu z4jt-j^Nc^Y-g>Je2PWZ+XKW-EK+4jr7j8n%n8gMuF)k4*!z7(L{l<Q-?>8piyi0sy zQ-`z|4<S6pxMn(LHEPtzse;3*0!}KTitKD208d<zzauHpDM&_0TYhE%VfhZGLm?<O zq%fp5q_Suf$PRHC(SGpY!D%z8Xw+TM>fj~CR&~V{R}{2ckNf>HS!0&}!V51ryOnuc z=C2FT{y;7`gAs$2S;4-2-+lKvvO$u1?X}nBmRoLdwr_tt5|Ou+V~+4<3cfSWVb8@m z1F6YxjCo9&Au~3JjLA3oGyNnSEm0K>OlCsDxES`5v_zE^u*@=YDgk9xM}B^X1c?#D zgAuYi>#Vc#Q(b{Qm)YN#F=L!WY-+#wJO<40QBA%1=9|u#&a-SKZwP$z%{NXO!YLuU zf+UM11%YHH%|J9rJ#1N(jCOa(jlM&D$fq}pWOaTr{9Z^!z#thKnoMm(s>3+Vii<b} zcvWLE-9w_=ikIaXXPhBTn>Ljvo_HeB&B@pG|EPPRrUp-zH3D|tDjJ9vUwlyp3>YAf zJ@%NhUE~Xd0(vsGpsTOG+DSf4UQyNMq#zwZG*)&+ApG**u5AweFU)3$N5Bc><BvZM zeS5>!Ky>Q3P^0p=^2#g2*1VK$59%P?lkswWPGcOc<P8B-*Em&l?AS5KB*m4Cq_W04 zE<}Y`>g`DAR<R|aXGQwpZ?AXdxI`56yBITRM>GmAJdyvzgQ1V6ek(ag00$YeIOFw+ zXPTAxA%G17FIJ5COct!<4gtLD@S;R*7tO@3iB^JnyIAhfh~CjLw^so$F;VZo|GsmC zz?K9#(V=+P;nV<eAugl#!3!zgg%}=U9ndYGfByMVxjb&B1od_8+SPdjo_F4PQK>i8 zE5S~M`iY=PmWZ(uUj(p^v~1bZ*;@&~Z`G=mb3lr3E0XtWWTg21`|suY>#uj7z}eji zQWx=T*pz%csCNKk+(fg0!`HFL9_#Fg+0_-ReW)XGx5pci*ej$y?8k)g#IwX)qVhz= zjpBjAhArZeM;>v(Z@6`1P{d<qg6928L_q>#1_Zf87G@Dhf_W*_$xBWKd6GO5{cR<C z1b9{m_8vKMq?<I5MZrq^5kRWy*|Vn$2sIU8&haO3mPr<#eMqIdD`d987K@4-_i;!L zlL3=@2m;SPLSArmNmL<w1xtV6aSe$Q<4C*WUUYDXX;HOTpf1~il2h7so__jioH+tD zG^YK=e$CdK_IJF$X#7FCOR85*psDWjwaTS-Y``k38lM%Ybdaw8kpJ=PuhTV#X{Rqh zLYk_sxPhup0$tZE&=)x&ZLo{(L_o8*1WH_i4#0uVstuOiAfOZ6pnw1VLCu;q1GQ1_ z=I9_$EkeMU_`m}Xq#gU@lTQXJX$6|RJMA}Bhr6*f?T0vjx#^~x0wv&pv5_`2W~=5S z&aKedY{Maw`^Q|wLY8f(tDX7OywEgJYW$ARr7zmcE}ssDD`aGjn>ux>n{xYOi29v0 zX_6Dg$c5yKLgylYky$AOHxLevQ7p2cUt%9oddRhr*GC_T7)as!@ZrOq!3iJKysneL zJrWU2kkC_4J>^ocN4Cel|Azqm{=fhI&-I(G^X)|O|MuFqcq4%7gKVYr3oD}Xj#<eX z0qh-QdhO7mgRo>oP6`q39PbO`BC46laS7YVsi&Um4xtY)fTJVCp5hC?E!6ZA<b}(u zpQx9@xJGc6FaB6QvCSYlc>_DY5#&g{sG1mWacyRt_HK>XP{<MG>-Y8Y$@tBNii+#z zx5V6N#b8a~MotPZ2P-d5P@N!2sE~mnEo5W}1CpWRu)Z^>4H`7aDNm74bxPuZLH7o+ zKxmKzVc{s^=^%7Wr0S+Ks2igQdPqp^)mLA2mVA==0WLBy4+AMMaO=x!3JN+GV;B!I z_eA!}M5a*ANk1gwn28f7%7YI+=z`KLs45XXu<JE>Z~OG=<Mj6E1j#CGWmg2SZ44VW z%#B4DzjIPBQsu;R$d47zJoAjx7ZQ|3azVy07WsqJ(4Uj`0-}N(7y}tgndsvo!x+c4 zc*h_X#!%id3{dDHF|sisft;Aklfr@otGF{lRQyghTZ9d=@N!MOZy-0WiJ=)XWgkmS za4h#n9k@Xgc}#RUcA&hLm`hYSZ5UuMQ(#$LZWI7;r6ZIc#)gytnV=iIFbMGm1c;E0 zPJmpPk&x*Wi=@id&Ok=pkRR?j&6+iH)??C3&?ywI3sKFTJJ<b9-Vv^Y)sySzH^ndy z(U2iS9Ptsf5;<dX7cAiaAt{sMgAn@LZ@;~OqY<9lXM)zFM-Ml!-F^4nP7@KH)`g=b zj3F4{(Z-+&AX#X%3#5dBi!qQ%xtF>y^fSgG$+6fKsYfuHVf125B<UaHB$Ie1_Yelg zb0&n;L;2*BPh1YO8Z~OTbQz4XxJ3KhO^_MmAS46f(H17%kR{|tS`Nlm4D(>X*v2&x zI7l&1JDB)qCUxZor_c};Hw0KD&7vb<{d3PfH!c4NzMrTfARW|pbc$E?G4lg7TnbM= z{dDIi-mP0V2NL8%-H;IjB7_8RsN-OKnE#YhPH~-x<VC@fNZuvRL(Gjv8Vr2b@3`X* z_Ym=*SlNjHFB5|xvr^W>wghzHdNP=qUwiGfF0BHSEGCdvb|Qcj^8EA9JB<%ZLgZBC zms|WGKgJITgRu@R5feM|%i)ekTn7_x#!raEn<~`c>oh@(m5irI#AtW$<RI%e1@~)6 z3o{96OVl{&>SYY%nj}Oer3Rj2?1%73$AemsdKjxAF~|z?Li5CiCW&j3l^1ttChPer zp&=@60<o3OhaeykU+5szX52h80I#3rSjyJHof_)Uzta~$8J&6nngX%0K|*vQg#}_* z|KPIAE^{v-?mKh_B_s25F7;9Tr~4zH|3Rdv2^gdi>kE2e@&Hkhp?2WFfliVw=$M4@ z{uzW&k2CWoFOYv1!`Rk%wTUr;M1HvRG12bZw{OfZ4LSHRiwQbpM!%o|!c2nNkIA_o zv-o5~DuUD~jEPLt{g{VY1mYnTJh2Mw$EA_NF^r$ofr}!AaTE0)?I0+X>!AYV{bj8t z3fMqWOn4aAX*bu%Ps!iedAu@>-(B$p!*P%kqU?tse&{3}RD6J)u>eq3g;_h|Izdz) zeDHxIEEb%KXMsp>yzxe7lR;%bO6!7JcnRo_|NFoHbF;5_yyyiL>uB6Jk^?z|u<fGK zW`HWlg|UbUClZIc|3yUulO3LnZGQ8|K_H~YK}Cr31Lg}Psf^<w$3>2({(L@9Jn=-= zpO6V;qi%m`l2_z${yR<**r$-Z&`Mz&Lsg8n3YDNYanJ^?8?6#|qJpG6QTflvoAJzx zBt_wdQQz?};C;gE0ks@}c(on%a9(76{=5IgmW7y18VQn|0XD`^UpUSm>wn|gz8(Iz z{C+aKLGO$`4Dpx;zMvH*9?V3U!4Xsfd0PoL0@y83A))R<4fN1M54j0sxLX~Y)?ph* zrO&&JO3ph!MB{21$Dr)$MBT@Dh7Agh5kZOclP_7-6*<=7#n+%g19?szXP7J!oy_Es zl5tGsH^>lYGbFgkg7+ZyDT=?Y|BO5?uixWNRIn^MQb+*ZXJp~VuwTIb0O$+4RDQ?P z1>GvDB4!ZSqWsd8KBA6usE2cywR?AUJOVNJ0X{Y@yj;A?C8Wpo{53frH6+(S4~w@; zv~n-eK6K2?#_*Uy6<LrZD`w)@>mX|+OuUNx<OFQ2gckuEE$|#;u%iDk>NDXhNQHqM zZ3xdSB;nOv(ZGOlkN3lyHzMogdR|)bzhmQtFffx)ATGvMuco6O#@UP$?nr5v2|N-R z*%0VcQkGzgqA(76^`C!7S=$5$4Tz2Llw%o}aqfVuXbacl+MFM)QbfhQfzhgLcl7`? z#y7nDc<9laLKf&VxhXtIh<8F#!X*<OC#fVj7QHB>#NY;rAnD-ek3$9ZqMt>4<-y^2 z(wn18XSRbR1_@zIq(0m!=`0*K5S0OXp}J(!4!<NPQX3u5Yz1OL8jGB*?CxH7x1ZS< zMpD|s42GAVnN2DeCJlHBp~hvhKp*qKGehG6=LLx7!=1`lCgR?hENXPBbAn;^j_sKD zisv{pReU1a7|UxCW`d25@rgJy)L+pkI4Cdy$0UKX7~^8qs#V?C#CV6NACq$&%6MlO z&%9R2$DQ(SF(yJBNH!1;lWW3!m`u=LjCE*mn6#7I4($|UALNSz1;odgiRU$0i&67_ z^7$UmSN6UD+TRct=Q37ePGLMHga~hJ+Cp0yLlFdMFVZ(*LbRt!l`2trPU5;Y2;_oG zBsb0=#U~Ft#Nz??0~j+6ZqyqRArEx35E(BCvj{qv@3@ZAK@50!@o^qxz{7<!#{<B% z01vWa18{!lfk8@RM!>OjEDr^R*e)IduHoAejfXU{4kXNhI&UBjf<$7(A<%|YD@;h3 zjbe;Ow~g(ChaH_aea%aSO@x5}@}_UBga-lMKRnO~{XliwrArsLG9wN`go&W0=e<kC z!nj2|Ct3lo8Hwfu(|hOfjxaVcCgSObBm+4yJ~8&;h{QOARtAzmf`RO?yF(&)-yz}f zUg1Q>#26xBT*O&|bG-z{`Plu@O7R=_g;d7a!+6JdiEx1lhOwExr9BWT`w%|34&P}{ zvu4elus~f17huL1%RcYak*{Y%2WTq^Mu1#4<YLkXDNvh0pk$ul7De~+I(rLrFbh=D z4b+*!<!a#yI+F`Dt1sR#X`4RWff81rL>B0v7~O_gZFxdKXUwjjbXFaxohr})&DEm= zr~6w6)j++#0#zyHss1pZq!Z^rm+B3;M^%9aIynw=b5pi$ND@{;fPi||xCCG-eg`U^ z2P!^?pjG*6g4+86bz%wBGbm8CK%hG9bQP)u8>l@n&>34AQR(b2PzgKG8Ec@S3h9ow zKjM#o&e#Ku-wD($I?#+SfzIO7H3HI7X(LdVXO}dP#C1B$PIpYQ{-IJs(7AKxK&AUY zwH<+Kagy~`lD^$WR3Ym^)h`Ea+O!GOb`xm!hCp@CA!}jInxi`LK;tw5X5l?G^Qo#J zGCh&ZwA?RJmFngLRXqe}opn~A_QpU}2$`N^KgI`vF=NKKNfQ$dwU=ajQMG$Gi36!w zJzJAnE1+fsmra(*mBy0=s@@EkbXiFp0&#V$$d`_?fSim22S!PB)p*$WxG#*A7+J$! zu;~#3fIW)r26#APd&8)jSwa7euAOu+q_9$wa>ihsWUya(@21Sowclcc0G_Xe65yqZ z!w;T*yUWIaJ%acg?7SG$@hZdiNPsGiPVp?gw;iv+JUjCO<mbA)dpH8I?mUhL*yu<u zYtLkUP>sU5iQ`MVa&`!yN<nRgj+_~*X1fT~y**GPY<8z5Y@ZTTpjqjiT?*a4l2wlF z7f4=6RY+mnH&Uf~Jp~$I7^nwl!tSXcURbkI1nM~9R9~9bEKmofT*06Q^+2730(HCy zbOPoiWh67y++i-7yft9$3w7`b)VU{k^wCEHoq&b8$pxxyarFR<0<RXhD)5LT{)!nZ zk_!ncN!&|17at#%NKi<(gZHAUjGW6VQR~Fu5Xqj?Sv8)SS6y|L^FqSQ3Qr_l`B{zy zFDAUpkhHAC76IJQ$s&ULGC`rF+X4T)6#5izPo7<#VLZW*On7di;t`gYMrAlhM|u~Z zgjXXj+GsS0FCv^JnjU@`E^B}00RbbbJb;#nQ*g^?Ab}Jh2U5Ba2f@Zmg*q)>h4d-} zAgWV@i`BqAmOODJ7sh23H`v@1+*-Lm)PW3Wz7VrPW(49zaN%e0%<I7E>$J~Z5g<m2 zHPvv(M*T{nT9Tng<C&sgiRmHE$3M4_6&YK6uDM-X6x&}QPGXE$$-!q{pgj;ND)^jE zuwvUvt346{BdSO=lHfbDU0yJB=>*y_m=Nng3<c5*-y@U$M(dUsWC)}tMuRvG0{)2o zfS^e0LOcg?8PP7>FOn6rUIr23u?S$a5*-A1mat8cl?5r72>~&V(Jnlz*sxeef*2uU zA9;4LQS$6!kM*h2qJ3@ZBar~lE(Aq95VllEnFS7s2_u#-nt}eYxQBvVrx8_>_CDD< zSQL#JAz4093lii<)(4Eh3^r=pazTLv1`_zk{@oRV`3Ur6W9A7V5wML^!*7ri#&TY8 zUT&5fAV4veh2Thx+boWWG{=H}-Me>>SwMCy?<I@6XI3uxPBeT0vZy}$p9!P~Thil@ ztsAmZ7wwq$E9AkmOAsl~GS3$o5D2~}^9&Lf-^tRD*MNAWbR>Q<(lF5?FqO#<zd@F~ zCp^ncf|%q*LyF8EZQGk)1dO!ui(m-OXST`<$qbfHUQoz~!b`}@iH!jJ7Ag)l$OrF2 zY(u1B!2^tq>>s3B;Wza85FMn%?3b4sLS!(Bg(S5*C`k!}9Se0Rk|cvG>MAQaM*z}i zHEb5-BU=ipFIF7@&v+KR22l}`iGhvp5DCv5#Dy0k*=g`HM6C>I@yz;-{B%6K*qGRO zcFDfL<c-M|&#=!4%5zT!2(qG}GUr~Y-%1i8&~V7){>D~G)L}VkDs01)nYT7lR6_T` z+stUdn+}Wd$*6&kF`xM4B91Yog^l>mC%IJ+-NK;ECyy2h(lD@)kA_8a5Vz1LS;-m! zL=BS8fg9|=jDvL?G{7z4{mQe4u9;ju%(QrRVJ@Cu*p<9Oh}q1XcxKrsJj<NR#>@-x zkeL}eKAvG%4mATzRYhYb@+zifjjHX57Xn69@j^O**TH*%%%hMPQ&EV?lM{r+hQWX- zD=#@73^~yoCh(TrG<?*kQ7$-(0SnI#+z+Ff_BxUG7p-g4_ZW+C$wTMH;C;?H=Qx5# zWC?3Qtmm2alP#WIR0V9@lb={YP)x88#n5*%vBGF;VpCXqHgD}RqRJa?c2)=wfNa0~ z@=IrIK_|%IgkO87PMw^tG6YRbSQF5R;mLr%IC@hiAb3xp@56=EiXo6p2pCZ%Q*VW( zt%IJOFePEW0NI0p!GiS-2=ro27}S*n%b+4GXzqgqH-Q7b;-tLB@PlCwbst6>D~3Qa zB4DJIjJ;-U&J+P`8%SFW(tM&W!j46rAc!j#KK+?CB+fsg1|^;VX=;K7zG3shMnf<Y zaRvB@TQLNZ837}zWbQp{d!`6r>mZN@vO?0rNRMp>A7uy&^1?Qk>;<<^?va!WxPL-e zI7}ctY1(r~R>^*k)_y|(1dOQcIY?;)2&%ze!+_3Ujva^98zCzKYglD2Sv<pVCg6Su ztYhPT@Q%Yf6Jp|iQ19VjV8swfB?OG9QmHrXUa~}hKpU2JM$#hk6t`$>Tdc)RpbhFV zHlk0%l+NC#3n8rp?Wf&%aUvnoUY2boSct$kD~3R-B2Zw6YS3}BmW0$7AXT5Oq_{6! zp-E$aq=KCXBEZ%~+zxKf5CHD)tkXhV4zAv@&cowVEkIIiqzoZ-2r5isiy%@67NSCe zqJ7w;Y@Ldvc;u{+`9z?=kXAl%veQ!(0b+P?M1ZUy2cJ_4X$+MiM1We&r%%BB+i%37 zp+Y3KhtwWER?P>i`XE)@>mee@ka~#~B4aFsNs0v@Lqz^8)Oy6a;et;fHe>{0`e0Uq zvcn$xmZFb}-Btn-FrrEz_;%^=Apn6O#gO&GXZ|309ls;lpn}7*FftewX^Rbl@|i&R zj-!JQxaJe$!|8+KgReOcqJ#91m>@)?I&4so5+up7Rt$lHjDQhULGFFK_uV3ZRD=YC z<b^bZRKx~pv5{<!{rpBY1qg`^l7e9HG=z-!B=rxH64Do&|C`?+Dv~wEGS&EQZ`KYl z1VV~{5miV##+sEp2q0Z$Oj$+ik*Xjb)QA2vdG5+uY6#>Efz_WD=@L(003)rO^@|;4 z2p9r}fFY0|1a_t>PSC~evW9>mU<eojhQRI+FrwNWLbl%!Fa!(%Lm;6D7*Qn@e!F<Q z5Wvx)pp=4iuLY$~3;KN)E$9H){f7eq91~bmiM)1r&Jlw|><}BS+oTQ3O|n_M^~kVJ zeYk(KPE)cKyboDukUWOm4{LI<Xkcb#=FFLHnPl?+k)+m2wg{Z|$bQS>=?jo;V{B*c z5HRT~cX-*+F+u=uIo!5?`spXR{r1~kHVLvt;NDHv@bk_)&#h*g)5}vIu}s5;4Rhp` z)APb~9E1&dfBWsX^7!MAJJ<NE+K2l*#}Qa;#Slm}1WdY0wH{5<`$3{&0YEY{AW@K| z0V#ptS%PAbFop~nB1au{lpKEe;k&&{f@?`b<g-WY_Jj}~08tSL3}F+?#l2)zNVW9P zLl3#&;jE6y?e7Z~EO2R_nl)>d+f(g$Lm;{cG#oOyf7XwGbgyN#83MaS0NvMKd+p_d zl8B(C5JgLNR@U*DJ9n;2FP>RNuv*5#xvaLBnGuJGU7~lhDn7b5v;Cas(@$i!KeM)E zc3j5a$gPA<G_sHY5KLgI%#!~3dn2;AVZ(+lCzJop*Fz8(_nTGWni;Vm2_dtCS+izk zb(;Nd2*eEm6RY9|wOuGD2xJ^&{W;_~Ld;-kTVFVq)Z-H;PIM{L5y#Fr;|#Z|AjkN+ zIL{Xb<<X-@rzPQo95253Vwc;9?~vh#AAac4ts_=Y$7PpY=1wOq`{$p3E~{6sb_oDU z8VG?RzES+S4Ep3|LL52s%ro7-?EafKZ=SsW{`*cO;yy7@oqzuMPMoAo6o`g3K<>Qr zPFcEisUz>+y?Z+`l(ZQvUIn?3ZPy#<NVW0ad+#}sj4Zeq@A~%b>xhp!SvHvc5TQ@K z(W_T4muBOI7haGz-+a@FwVZd{amU#@BY<WlAqbd6l@QqNqPs%?0}X>T-VyiRcb|On z$tTjSTQ@iOb1npeN5RW4zbwt0H<zYOo4Vhwzy5l+pi^e`Lo_TCOlH_tty;Oot5`nw z)?06Nhm$#$?^CBvb?a=9G4{FVo^wRV0i<Go>Zzxkrw95;26&F8KsL`l`>Zr+(!>!Z zIg|SL@9zl29gxL;NGyW}4RZOF&}~9`5aiWYU+s)eNKu6<9D9_?i&dmutLB}RbRiW8 zx^d&iQma-i*Ty^UxWf?=QY1t=a^y(2_6e7TkY0Q3HJ5OZWs%Xdvj4#cA9SsP;4y?D zwLwTs2H1b^z4y9&P}QncbCPZ4%9Wk&72}~5Lm*KI7-=O6dAn|=2p|cewt}Rtyz)vX zWk6gbMvRbw0|%z9jL5(|YSbu4Tq{?ubOSl6Fcy1y>#esktsBV&!w%%dcO)%H0V!hI zv}w*2k>q^1)nP1xKv2!GS|bI|1y*gufCO2bb=FzVxaK9cNs}fy!XVQv<N+ZerDRp8 z=k2%Ob|l-iYgd6BAm7fNJG<*Zm>BaQXw;b}oph2Q{osAly?b|g=bd+)B*;FN1Lm6E zu*Lm6^UO0&y+<})3Yfj}$}27tFZV%vk;J$zB#snG##{=Ec|lG-#k&!egT?+!5CTS2 z2?E|On>hju%8(UAz>>YV1wvFUTC@;m_rBovhUyEi0$l#EZy{|!Xbk9?6{HqaWp(P* zal0WU24|!UNDiq2GGv|mv17-|xN+m0ga@%v7|<EWA*e?mebil-<#>_SzWVAbsiIyG zv;%TMQlc##J9c#EX7(RyI#igfFA}-JBhnA|`tipfr=12-_!Sjtcl-A3-Ss_*dfS{= zc_Q`EZiog<Y4_m4gXNZ6ZgH7-Igfgf5GiHKl#xDt`nZV!e<R&;9-meWfkY!<_Nqkp zfL(tV1Rx%Wsz;9=PK9;jjW;@#npahE0#X+QfvOII!kK|VngQPr)(o_f1$7t&H$qlT z#0CVZc!vcBWfri)<?xhKPH_V@`+PkR2&9I340RzvVMsR&_DEY0E5wyqTO!-xzjF^r zgPtt>?~ons!!G6RRkR_}IRavVSgC`;em3r%<4|eh3W-ySKaYC_+r^6)J9`>``@(q~ zZ^aM@BLXH-g|Sx(aGN~I_?bKo20eTBl&7D5TJE~*t~6vAa2aR`CZZrwc&|Y(W%#=M zNeu4TdT>yHfH;RwRClPsklY3g7~rG}26)t8T*KFcS7816^=0_*;d0w;x4C-mzWZ)R zjvVK0SH3L}2R1b%r_2f}I~J=#4H<b1<V1Z)a9)*{@qEY!QuflJ-_JEUmz5tOIwVMN zVFNSn;pd-!b`m72N$~K6IyndO<DNb7`Th232qX#tBdSCppW@fO<GjM(cwd0L+$w`G z12%)DFQl!w?z-!o^8(HmFTVJq+XVsP(16Vb=LU#}fgc+d12u*EsLK!hRjXEYQp-K} z+~d}*=URC5K|&1Zc=MsoBPyA~8uv&kzMw`#@}eGWX?WvdA483Yor!(45gQlefTtb5 z;c$^zK|1Qtp+lNio<9aD5XXm}O4bxe1VVufeW9(?i}zz>@z_DS)Pw2|Pe3LC5Hn69 zNUlhvkQZt}t_690@WBT%K_jmraxmi@uL8wG(TX9EC<KZO)RL%++jR{Aj{ro2eTIP) zLSTR-G8pND0Ui4d13$+wU}L+%RyAeH6z9C)?Mslx>8GFW2nr_xh>gLUff;E9g2R!* zJ0<-1<B!fO5chK&9k>QIB%}<^gUBEwT(|jq=+L3AT}VWD<TY#7%-Oe4M?!4RKmWY5 zb<s|U5+cefM-)<!uNRV_ey$6-ky{F93#1%K1b2BHBp?rRNP!VV=A|nL6(<eu5syE} z7J|cDa?F@9j)bTiyB{Qm;|SyfK3p5y9O_8apJ0S4lWX!jg?{h^?)%I>4S{$eU<+5p z3u(Jfwg@0GEL^zIxqlO6%b<xGj+7M))(pC+{E!kD+`Y#eWC2;>-3GB8ee}_8P^Avk zdf1;(iE%t+0s$g5p`L>%>ej960%*A&=dcg;7<FNvB2Egj!dU}H1W#IC(ts>+%0N2f zdZ-v_Bg6_JP|xlPk|6a$LbR2(;1I!g3Qi=b7`5>6Jh1<A-4JpNFbLQ+VK6xzZ4 zfC17M&K{^Sk(MAuh!0OdBr1{(VsC@!C`=qCPoC^NBkR?x=gu=PKnIQemjDEes1g9W zT{8LzKy(mKP74Dy#oMAH&tqTY?-~Dwlo-gdH)U3G+ZJC3WX!dY#Ih<#I*}?se;l5Y z7hG_Gbm`JXh%t*SwAZUYBY*ebv)iBX-u*fDX$Zs<0rLQiC)#$cm?4lGlA;ljM3k{n z8TV!Un?XM-lA^BMw#8o`+tBVxBuOC&z5e>^PKtt1Xcwxzj3qmg;@Ir=XT0}}b=%*D zKs*pIqWTZzMvo5elpkM!|7x;thCrMVK#IfOhJEeC6Hk=OFTdQW9^>2$yPP2qa|BEc z7;}j20){|r5Fqa$>79r(!%+eo8MZDfhCr$zU__N_J!<!32p9r}K#Cz?@l`3-p>{il zfFWQAq#6Q7RH@dZc0Y!IAz%oE3xSqHZqPiMJJ$%vcgu{l@(rO?U<eojhJYauO9YIl zVu`NRZ3q|whJYcEZv>2}@(rO?U<eojhJYauQv@0gncUylDy9%y<%WPEU<eoj`9;8p zD!&NY`G$ZYkXi^lcK(67E@qT`0a6Q^Fx-uiRv3E0+F%G60)~JgkR<{}R9Qk}I}8Cs zzz{G5!hnDgRTz4~+F%G60)~JgkR<{}R9Qk}I}8Cszz{G5!hnDgRTz4~+F%Ie6M+V^ zUeU!w%oiY^Fxu%!fPj%!5_FO^(GV~M41t1-fDu(e?tQy=L%<L)1d;#&BdR3mBx|A} zU<eoj1sMS&s)F46cJGFOAz%n30Rl!;Nzh3t+Qb31w_akt04dtJc3Xx(tPn8LiWRa} zuOVOv7y^bsR1h$tiVBQYi6LMJ7y^bstPn7wiWRa}uOVOv7y^bsR1h$tiVBQYi6LMJ z7y^bstPn7w$`!JAmU_B`$icU@Yt_7Su1>Ur4FN;I5HJLyfPj%!6hO2p3;{#H5HJK{ zhky}P?69?(4FN;I5HJLyfI!jFqen-f8ml4_fkK4}Ne~2)KjrGXop1OR^yh~cCa)Oq i{@D4cg`f6l5FB`GyJb!4FC0YYwrky?)mtrk4*Gw3%MB?2 literal 0 HcmV?d00001 diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh index 40403654..d69dcd02 100644 --- a/include/hpp/manipulation/graph/edge.hh +++ b/include/hpp/manipulation/graph/edge.hh @@ -376,7 +376,78 @@ namespace hpp { WaypointEdgeWkPtr_t wkPtr_; }; // class WaypointEdge - /// Edge that find intersection of level set. + /// Edge that handles crossed foliations + /// + /// Let us consider the following simple constraint graph + /// corresponding to a robot grasping an object with one gripper. + /// + /// \image html constraint-graph.png "Simple constraint graph corresponding to a robot grasping an object." + /// + /// In order to disambiguate, we assume here that + /// \li transition <b>Grasp object</b> is in \b Placement state, + /// \li transition <b>Release object</b> is in \b Grasp state. + /// + /// If state \b Placement is defined by the object lying on a planar + /// polygonal surface, then + /// \li state \b Placement, + /// \li transition \b Transit, and + /// \li transition <b>Grasp object</b> + /// + /// are all constrained in a foliated manifold parameterized by the + /// position of the box on the surface. + /// + /// Likewise, if the object is cylindrical the grasp may have a degree + /// of freedom corresponding to the angle around z-axis of the gripper + /// with respect to the object. See classes + /// \link hpp::manipulation::Handle Handle\endlink and + /// \link hpp::pinocchio::Gripper Gripper\endlink for details. + /// In this latter case, + /// \li state \b Grasp, + /// \li transition \b Transfer, and + /// \li transition <b>Release object</b> + /// + /// are all constrained in a foliated manifold parameterized by the + /// angle around z-axis of the gripper with respect to the object. + /// + /// Let us denote + /// \li \c grasp the numerical constraint defining state \b Grasp, + /// \li \c placement the numerical constraint defining state \b Placement, + /// \li \c grasp_comp the parameterized constraint defining a leaf + /// of \c Transfer (the angle between the gripper and the + /// object), + /// \li \c placement_comp the parameterized constraint defining a leaf + /// of \b Placement (the position of the object on the contact + /// surface). + /// + /// As explained in <a + /// href="https://hal.archives-ouvertes.fr/hal-01358767">this + /// paper </a>, we are in the crossed foliation case and manipulation RRT + /// will never be able to connect trees expanding in different leaves of + /// the foliation. + /// + /// This class solves this issue in the following way by creating an + /// instance of LevelSetEdge between \b Placement and \b Grasp. + /// + /// When extending a configuration \f$\mathbf{q}_{start}\f$ in state + /// \b Placement, this transition will produce a target configuration + /// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig) + /// \endlink as follows. + /// + /// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge + /// histogram (see method \link LevelSetEdge::histogram histogram\endlink) + /// \li compute right hand side of \c grasp_comp with + /// \f$\mathbf{q}_{rand}\f$, + /// \li compute right hand side of \c placement_comp with + /// \f$\mathbf{q}_{start}\f$, + /// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp) + /// using input configuration \f$\mathbf{q}\f$. Note that the + /// parent method Edge::generateTargetConfig does the same without + /// adding \c grasp_comp. + /// + /// The constraints parameterizing the target state foliation + /// (\c graps_comp in our example) are passed to class instances + /// using method \link LevelSetEdge::insertParamConstraint + /// insertParamConstraint\endlink. class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge { public: @@ -443,18 +514,39 @@ namespace hpp { /// Build path and target state constraints virtual ConstraintSetPtr_t buildTargetConstraint(); + /// Build the histogram + /// \sa LevelSetEdge::histogram. void buildHistogram (); + /// Return pointer on histogram of the edge + /// + /// The edge histogram is a container of configurations defined by + /// a set of constraints called the <b>condition constraints</b> + /// that a configuration should satisfy to be inserted in the + /// histogram. + /// + /// The histogram is passed to the Roadmap via the graph (method + /// Graph::insertHistogram). The roadmap then populates the histogram + /// with all new configurations satisfying the condition constraints. + /// + /// The condition constraints should therefore be the constraints of + /// the target state of the level set edge. + /// + /// \sa LevelSetEdge::insertConditionConstraint LeafHistogramPtr_t histogram () const; /// \name Foliation definition /// \{ - /// Insert a numerical constraint that parametrizes the foliation + /// Insert a constraints parameterizing the target state foliation + /// \param nm the numerical constraint, + /// \param passiveDofs the passive degrees of freedom of the + /// constraint. void insertParamConstraint (const ImplicitPtr_t& nm, const segments_t& passiveDofs = segments_t ()); - /// Insert a numerical constraint that defines the foliation + /// Insert a condition constraint + /// \sa LevelSetEdge::histogram void insertConditionConstraint (const ImplicitPtr_t& nm, const segments_t& passiveDofs = segments_t ()); diff --git a/src/graph/edge.cc b/src/graph/edge.cc index 6545f994..d59e75be 100644 --- a/src/graph/edge.cc +++ b/src/graph/edge.cc @@ -203,7 +203,7 @@ namespace hpp { return targetConstraints_; } - // Merge constraints of several graph components into a config projectors + // Merge constraints of several graph components into a config projector // Replace constraints and complement by combination of both when // necessary. static void mergeConstraintsIntoConfigProjector @@ -289,6 +289,11 @@ namespace hpp { if (state () != stateTo ()) { components.push_back (state ()); } + // Copy constraints from + // - graph, + // - this edge, + // - the destination state, + // - the state in which the transition lies if different mergeConstraintsIntoConfigProjector (proj, components, parentGraph ()); constraint->addConstraint (proj); @@ -654,7 +659,10 @@ namespace hpp { const ConfigProjectorPtr_t cp = cs->configProjector (); assert (cp); + // Set right hand side of edge constraints with qStart cp->rightHandSideFromConfig (qStart); + // Set right hand side of constraints parameterizing the target state + // foliation with qLeaf. for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin (); it != paramNumericalConstraints_.end (); ++it) { @@ -778,6 +786,13 @@ namespace hpp { ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations()); + // Copy constraints from + // - graph, + // - param numerical constraints + // - this edge, + // - the destination state, + // - the state in which the transition lies if different + g->insertNumericalConstraints (proj); IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin (); for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin (); -- GitLab