2020-06-23 19:37:10 -07:00
|
|
|
|
B
|
2020-07-26 22:04:23 -07:00
|
|
|
|
¼N _?# ã @ 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 22:04:23 -07:00
|
|
|
|
Z
|
2020-07-26 00:42:09 -07:00
|
|
|
|
dd„ Zdd„ Zd d
|
|
|
|
|
|
„ Zd/d d
|
|
|
|
|
|
„Zdd„ Zd0dd„Zd1dd„Zd2dd„Zdd„ Zdd„ Zdd „ Zd3d d „Zd d „ Zd!d"„ Zd#d$„ Z d%d&„ Z g d fd'd(„ZG d)d*„ d*e ƒZ G d+d,„ d,ƒZ G d-d.„ d.ƒZ!dS )4é 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
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
|
|
|
|
|
r ÚgetAABB s
|
2020-07-26 14:44:35 -07:00
|
|
|
|
8r c C sD t t| d |d d | d |d d | d |d d gƒ¡S )Nr r r )r ÚsqrtÚsum)Zpos1Zpos2r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r
|
2020-07-26 14:44:35 -07:00
|
|
|
|
r ÚgetDist s r çš™™™™™¹?c C sf t j | jjdd… | jjdd… ¡}t j ¡ }t| |ƒrBt| ƒS ||k rZt | j¡d S |S |S dS )zbiased samplingr r
|
2020-06-24 13:58:07 -07:00
|
|
|
|
r r N) r ÚrandomÚuniformÚenvÚboundaryÚisinsideÚ
|
2020-07-26 22:04:23 -07:00
|
|
|
|
sampleFreer Úxt)Ú
|
|
|
|
|
|
initparamsÚbiasr r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r r ( s &
|
|
|
|
|
|
|
|
|
|
|
|
r c C sh x | j jD ]}t||ƒr
|
|
|
|
|
|
dS q
|
|
|
|
|
|
W x"| j jD ]}t||dd�r*dS q*W x | j jD ]}t||ƒrNdS qNW dS )zsee if inside obstacleTÚobb)ÚmodeF)r r Ú isinboundÚOBBÚballsÚisinball)r r r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r r 6 s
|
|
|
|
|
|
|
|
|
|
|
|
r Fc C sX |dkrt | ||ƒS |rÌ| d | |d d …df k|d d …df | d | k @ }| d | |d d …df k|d d …df | d | k @ }| d | |d d …df k|d d …df | d | k @ }||@ |@ S | d | |d koò| d | k n �oR| d | |d k�o"| d | k n �oR| d | |d k�oN| d k S S d S )Nr" r r
|
|
|
|
|
|
r é r é )Úisinobb)r r r# ÚfactorÚisarrayZcompxZcompyZcompzr
|
|
|
|
|
|
r
|
|
|
|
|
|
r r$ C s 888 r$ c C sä |r„| j t |t t|ƒ¡f¡j j d d …dd…f }| jd | jd | jd | jd
|
|
|
|
|
|
| jd
|
|
|
|
|
|
| jd
|
|
|
|
|
|
g}t|||d�S | j t |d¡ }| jd | jd | jd | jd
|
|
|
|
|
|
| jd
|
2020-07-25 23:58:53 -07:00
|
|
|
|
| jd
|
|
|
|
|
|
g}t||ƒS d S )Nr r
|
|
|
|
|
|
r r )r, )ÚTr Ú column_stackÚonesÚlenÚEr$ r )r r r, ÚptsÚblockÚptr
|
|
|
|
|
|
r
|
|
|
|
|
|
r r* N s 2@@r* c C s& t | dd… |ƒ| d | kr"dS dS )Nr r
|
|
|
|
|
|
TF)r )r r r+ r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r
|
|
|
|
|
|
r r' Y s r' 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 |
|
2020-07-26 22:04:23 -07:00
|
|
|
|
d |
|
2020-07-25 23:58:53 -07:00
|
|
|
|
d |
|
2020-07-26 22:04:23 -07:00
|
|
|
|
d |
|
2020-07-26 00:42:09 -07:00
|
|
|
|
d |
|
2020-07-26 22:04:23 -07:00
|
|
|
|
d |d k�r\dS dS )Nr r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
éÿÿÿÿr r TFr
|
2020-07-26 22:04:23 -07:00
|
|
|
|
) Úp0Úp1ZballÚcÚrÚlineÚd1ÚtÚd2r Úkr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Ú
|
2020-07-26 00:42:09 -07:00
|
|
|
|
lineSphere^ s$ ..22
|
2020-07-26 22:04:23 -07:00
|
|
|
|
:
|
|
|
|
|
|
.: :.: 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Úabsr1 ) r6 r7 ÚdistÚaabbÚmidÚIÚhlr- r9 r
|
|
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r ÚlineAABBp s( ::4& ( ( ,* ,* ,* rG c C sh |j t t | |g¡ddgg¡j }|jdddg }|_t|dd…df |dd…df ||ƒ}||_|S )Nr r r
|
2020-07-26 22:04:23 -07:00
|
|
|
|
)r- r r. r r@ rG )r6 r7 rB r" ÚresZoldPZansr
|
2020-06-23 19:37:10 -07:00
|
|
|
|
r
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r ÚlineOBB‡ s
|
2020-07-26 01:23:46 -07:00
|
|
|
|
$&rI 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
|
2020-07-26 22:04:23 -07:00
|
|
|
|
D ]}t ||||ƒr�d|fS q�W d|fS )z see if line intersects obstacleNTF) r r$ r r Úranger0 r rG r& r? r% rI )r r ÚchildrB r r
|
|
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r Ú isCollide’ s
|
|
|
|
|
|
rL c C sX t | j¡}| jdkr | jd S t|t|ƒdƒ}t jj|| dd�}t| jt |¡ ƒS )Nr r )Úaxis)
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r r ÚVr r r0 ÚlinalgÚnormÚtupleÚargmin)r r rN ÚxrZdistsr
|
2020-07-26 01:23:46 -07:00
|
|
|
|
r
|
|
|
|
|
|
r Únearest© s
|
2020-07-26 22:04:23 -07:00
|
|
|
|
|
2020-07-26 00:42:09 -07:00
|
|
|
|
rT c
|
2020-07-26 22:04:23 -07:00
|
|
|
|
C sž t |¡}t | j¡}t| jƒ}| j}| j}t|t |¡| |ƒ}| jrNd}| j dkrd| jd gS t
|
2020-07-26 00:42:09 -07:00
|
|
|
|
|t|ƒdƒ}t j j || dd�|k }|| } t | ¡S )Nr r )rM )
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r r rN r0 ÚetaÚgammaÚminÚlogÚdoner r rO rP )
|
|
|
|
|
|
r r rN ZcardVrU rV r9 rS ÚinsideZ
|
2020-07-26 00:42:09 -07:00
|
|
|
|
nearpointsr
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
|
|
|
|
|
r Únear± s
|
|
|
|
|
|
|
|
|
|
|
|
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r[ 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 rB ÚstepÚ incrementÚxnewr
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r ÚsteerÁ s F.r` c C s. || j krdS t| | j| ƒt|| j| ƒ S )z-here use the additive recursive cost functionr )Úx0ÚcostÚParentr )r r r
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r rb É s
|
2020-07-26 22:04:23 -07:00
|
|
|
|
rb c C sN | j }x>|| jkrD| j| }| t ||g¡¡ |t||ƒ7 }|}qW ||fS )N)r ra rc r r r r )r ÚPathrB r Úx2r
|
|
|
|
|
|
r
|
|
|
|
|
|
r ÚpathÐ s
|
|
|
|
|
|
rf c @ s6 e Zd Zdd„ Zdd„ Zdd„ Zd dd „Zd
|
|
|
|
|
|
d „ ZdS )
|
|
|
|
|
|
Úedgesetc C s
|
|
|
|
|
|
i | _ d S )N)r1 )Úselfr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Ú__init__Ú s zedgeset.__init__c C sN |d |d }}|| j kr.| j | |¡ n tƒ | j |< | j | |¡ d S )Nr r )r1 r Úset)rh Úedger r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r Úadd_edgeÝ s
|
|
|
|
|
|
|
|
|
|
|
|
zedgeset.add_edgec C s& |d |d }}| j | |¡ d S )Nr r )r1 Úremove)rh rk r r r
|
|
|
|
|
|
r
|
|
|
|
|
|
r Ú remove_edgeå s zedgeset.remove_edgeNc C sx g }|d kr@xf| j D ]&}x | j | D ]}| ||f¡ q$W qW n4x2|D ]*}x$| j t|ƒ D ]}| ||f¡ qZW qFW |S )N)r1 r rQ )rh ZnodesÚedgesÚvÚnr
|
|
|
|
|
|
r
|
|
|
|
|
|
r Úget_edgeé s
|
|
|
|
|
|
zedgeset.get_edgec C s
|
|
|
|
|
|
|| j kS )N)r1 )rh Únoder
|
|
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r Ú isEndNodeö s zedgeset.isEndNode)N)Ú__name__Ú
|
2020-07-26 22:04:23 -07:00
|
|
|
|
__module__Ú __qualname__ri rl rn rr rt r
|
|
|
|
|
|
r
|
|
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r rg Ù s
|
2020-07-26 22:04:23 -07:00
|
|
|
|
|
2020-07-26 00:42:09 -07:00
|
|
|
|
|
|
|
|
|
|
rg c @ s e Zd Zdd„ ZdS )ÚNodec C s || _ d | _d | _d S )N)ÚdataZsiblingrK )rh ry r
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
2020-07-26 00:42:09 -07:00
|
|
|
|
r ri û s z
|
|
|
|
|
|
Node.__init__N)ru rv rw ri r
|
2020-07-26 22:04:23 -07:00
|
|
|
|
r
|
|
|
|
|
|
r
|
|
|
|
|
|
r rx ú s rx c @ s e Zd Zdd„ Zdd„ ZdS )ÚTreec C s t |ƒ| _d| _|| ji| _d S )Nr )rx ÚrootÚindÚindex)rh Ústartr
|
|
|
|
|
|
r
|
|
|
|
|
|
r ri s
|
|
|
|
|
|
z
|
