2020-06-23 19:37:10 -07:00
|
|
|
|
B
|
2020-07-26 00:42:09 -07:00
|
|
|
|
3 _˜ ã @ sü d dl Zd dlmZ d dlZd dlZd dlZej ej ej
|
2020-06-24 22:52:40 -07:00
|
|
|
|
e ¡¡d ¡ d dl m
|
2020-07-26 00:42:09 -07:00
|
|
|
|
Z
|
|
|
|
|
|
dd„ Zdd„ Zd d
|
|
|
|
|
|
„ Zd d „ Zd
|
|
|
|
|
|
d„ Zdd„ Zdd„ Zdd„ Zdd„ Zd)dd„Zdd„ Zdd „ Zd d „ Zd d „ Zg d fd!d"„Zd#d$„ Z d%d&„ Z G d'd(„ d(e ƒZ dS )*é N)Úrepmatz /../../Sampling-based Planning/)Ú
|
|
|
|
|
|
visualizationc C s< |d | d |d | d |d | d g}t | |g¡S )Nr é é )ÚnpÚarray)ÚxÚyZdirec© r
|
|
|
|
|
|
ú…c:\Users\39131\Desktop\github\path-planning-algorithms\Sampling-based Planning\rrt_3D/../../Sampling-based Planning\rrt_3D\utils3D.pyÚgetRay s .r c C sJ g }x@| D ]8}| t t |dd… d¡t |dd… d¡g¡¡ q
|
|
|
|
|
|
W |S )Nr é é )Úappendr r Úadd)ÚblocksÚAABBÚir
|
|
|
|
|
|
r
|
|
|
|
|
|
r ÚgetAABB s
|
|
|
|
|
|
8r c C sD t t| d |d d | d |d d | d |d d gƒ¡S )Nr r r )r ÚsqrtÚsum)Zpos1Zpos2r
|
|
|
|
|
|
r
|
|
|
|
|
|
r ÚgetDist s r c C sb t j | jjdd… | jjdd… ¡}t j ¡ }t| |ƒrBt| ƒS |dk rV| jjd S |S |S dS )zbiased samplingr r
|
2020-06-24 13:58:07 -07:00
|
|
|
|
r gš™™™™™¹?r N)r ÚrandomÚuniformÚenvÚboundaryÚisinsideÚ
|
2020-07-26 00:42:09 -07:00
|
|
|
|
sampleFreeÚgoal)Ú
|
2020-06-23 19:37:10 -07:00
|
|
|
|
initparamsr r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r r ( s &
|
|
|
|
|
|
|
|
|
|
|
|
r c C s† x€| j jD ]t}|d |d kr.|d k r
|
|
|
|
|
|
n q
|
2020-07-26 00:42:09 -07:00
|
|
|
|
|d |d krR|d k r
|
|
|
|
|
|
n q
|
|
|
|
|
|
|d |d krv|d k r
|
|
|
|
|
|
n q
|
|
|
|
|
|
dS q
|
|
|
|
|
|
W dS ) zsee if inside obstacler r
|
2020-07-25 23:58:53 -07:00
|
|
|
|
r é r é TF)r r )r r r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r r 6 s lr c C st | d |d kr | d k rpn nL| d |d krD| d k rpn n(| d |d krh| d k rpn ndS dS ) Nr r
|
|
|
|
|
|
r r r r! TFr
|
|
|
|
|
|
)r r r
|
|
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r Ú isinbound> s lr" c C s` |dd… |d }}|d | d |d | d |d | d g}|d | d |d | d |d | d g}d|d |d |d |d |d |d |d |d |d |d |d |d }|dk�r"|d |d |d |d |d |d |d k�r\dS �n:|dk�rš|d |d |d |d |d |d g}|d |d |d |d |d |d |d k�r\dS nÂd| k �r²dk �r\n n¦| d ||d | d ||d | d ||d g} |d | d |d | d |d | d g}
|
|
|
|
|
|
|
|
|
|
|
|
|
d |
|
|
|
|
|
|
d |
|
2020-07-25 23:58:53 -07:00
|
|
|
|
d |
|
|
|
|
|
|
d |
|
2020-07-26 00:42:09 -07:00
|
|
|
|
d |
|
|
|
|
|
|
d |d k�r\dS dS )Nr r
|
|
|
|
|
|
éÿÿÿÿr r TFr
|
|
|
|
|
|
) Úp0Úp1ZballÚcÚrÚlineÚd1ÚtÚd2r Úkr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Ú
|
|
|
|
|
|
lineSphereW s$ ..22
|
|
|
|
|
|
:
|
|
|
|
|
|
.: :.: r- c C sD | d |d d | d |d d | d |d d g}|d | d | |d | d | |d | d | g}|d }|j d |d |j d |d |j d |d g}t|d ƒ|jd |t|d ƒ krÚdS t|d ƒ|jd |t|d ƒ k�rdS t|d ƒ|jd |t|d ƒ k�r2dS |jd t|d ƒ |jd t|d ƒ }t|d |d |d |d ƒ|k�rŒdS |jd t|d ƒ |jd t|d ƒ }t|d |d |d |d ƒ|k�rædS |jd t|d ƒ |jd t|d ƒ }t|d |d |d |d ƒ|k�r@dS dS )Nr r r FT)ÚPÚabsÚE) r$ r% ÚdistÚaabbÚmidÚIÚhlÚTr' r
|
|
|
|
|
|
r
|
|
|
|
|
|
r ÚlineAABBi s( ::4& ( ( ,* ,* ,* r7 c C sh |j t t | |g¡ddgg¡j }|jdddg }|_t|dd…df |dd…df ||ƒ}||_|S )Nr r r
|
|
|
|
|
|
)r6 r Ú column_stackr r. r7 )r$ r% r1 ÚobbÚresZoldPZansr
|
|
|
|
|
|
r
|
2020-06-23 19:37:10 -07:00
|
|
|
|
r ÚlineOBB€ s
|
2020-07-26 00:42:09 -07:00
|
|
|
|
$&r; c C s¶ |dkrt ||ƒ}t| jj|ƒs(d|fS x6tt| jjƒƒD ]"}t|||| jj| ƒr:d|fS q:W x$| jjD ]}t |||ƒrjd|fS qjW x&| jj
|
|
|
|
|
|
D ]}t ||||ƒr�d|fS q�W d|fS )z see if line intersects obstacleNTF) r r" r r ÚrangeÚlenr r7 Úballsr- ÚOBBr; )r r Úchildr1 r r
|
|
|
|
|
|
r
|
2020-06-25 18:55:55 -07:00
|
|
|
|
r Ú isCollide‹ s
|
2020-07-26 00:42:09 -07:00
|
|
|
|
rA c C sX t | j¡}| jdkr | jd S t|t|ƒdƒ}t jj|| dd�}t| jt |¡ ƒS )Nr r )Úaxis)
|
|
|
|
|
|
r r ÚVr r r= ÚlinalgÚnormÚtupleÚargmin)r r rC ÚxrZdistsr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Únearest¢ s
|
|
|
|
|
|
|
2020-07-25 23:58:53 -07:00
|
|
|
|
rI c C sŠ t ||ƒ| j }}|d |d | | |d |d | | |d |d | | f}|d |d |d |d |d |d f}|S )Nr r r )r Ústepsize)r r r r1 ÚstepÚ incrementÚxnewr
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r
|
|
|
|
|
|
r Ústeer« s F.rN c
|
|
|
|
|
|
C s” t | jƒ}| j}| j}t|t |¡| |ƒ}| jr8d}t | j¡}| j dkrZ| jd gS t
|
|
|
|
|
|
|t |ƒdƒ}tj j || dd�|k }|| } t | ¡S )Nr r )rB )
|
|
|
|
|
|
r= rC ÚetaÚgammaÚminr ÚlogÚdoner r r rD rE )
|
|
|
|
|
|
r r ZcardVrO rP r' rC rH ÚinsideZ
|
|
|
|
|
|
nearpointsr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Únear´ s
|
|
|
|
|
|
|
|
|
|
|
|
rU c C s6 t |t| jjƒkƒrdS | j| }t| |ƒt||ƒ S )z-here use the additive recursive cost functionr )ÚallrF r ÚstartÚParentÚcostr )r r Zxparentr
|
|
|
|
|
|
r
|
|
|
|
|
|
r rY Ä s
|
|
|
|
|
|
rY c C sZ t | jjƒ}xD|t | jjƒkrP| j| }| t ||g¡¡ |t||ƒ7 }|}qW ||fS )N) rF r r rW rX r r r r )r ÚPathr1 r Úx2r
|
|
|
|
|
|
r
|
2020-06-25 18:18:33 -07:00
|
|
|
|
r ÚpathÌ s
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r\ c C s, t | d ƒd t | d ƒ d t | d ƒ S )Nr ú r r )Ústr)r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r Úhash3DÖ s r_ c C s t dd„ | d¡D ƒ¡S )Nc S s g | ] }t |ƒ‘qS r
|
|
|
|
|
|
)Úfloat)Ú.0r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r ú
|
|
|
|
|
|
<listcomp>Û s zdehash.<locals>.<listcomp>r] )r r Úsplit)r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r ÚdehashÚ s rd c @ s, e Zd Zdd„ Zdd„ Zdd„ Zdd„ Zd S )
|
|
|
|
|
|
Úedgesetc C s
|
|
|
|
|
|
i | _ d S )N)r0 )Úselfr
|
2020-07-25 23:58:53 -07:00
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r Ú__init__ß s zedgeset.__init__c C sN |d |d }}|| j kr.| j | |¡ n tƒ | j |< | j | |¡ d S )Nr r )r0 r Úset)rf Úedger r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r Úadd_edgeâ s
|
|
|
|
|
|
|
|
|
|
|
|
zedgeset.add_edgec C s& |d |d }}| j | |¡ d S )Nr r )r0 Úremove)rf ri r r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r Ú remove_edgeê s zedgeset.remove_edgec C s: g }x0| j D ]&}x | j | D ]}| ||f¡ q W q W |S )N)r0 r )rf ÚedgesÚvÚnr
|
|
|
|
|
|
r
|
2020-07-25 23:16:54 -07:00
|
|
|
|
r Úget_edgeî s
|
