2020-06-23 19:37:10 -07:00
|
|
|
|
B
|
2020-06-24 13:58:07 -07:00
|
|
|
|
¨°ó^. ã @ s2 d dl Zd dlmZ d dlZd dlmZ d dlm Z d dl
|
2020-06-23 19:37:10 -07:00
|
|
|
|
m Z d dlm Z
|
2020-06-24 13:58:07 -07:00
|
|
|
|
dd„ Zdd„ Zd d
|
2020-06-23 19:37:10 -07:00
|
|
|
|
„ Zd d „ Zd
|
|
|
|
|
|
d„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd „ Zg d fd d „ZG d d „ d eƒZ e d!k�r.e d"d#d$g¡Z e d%d&d'g¡Z e ƒ Z!e! "e e g¡ e#e! $¡ ƒ e! %e e g¡ e#e! $¡ ƒ dS )(é N)Úrepmat)ÚAxes3D)ÚPoly3DCollectionc C s< |d | d |d | d |d | d g}t | |g¡S )Nr é é )ÚnpÚarray)ÚxÚyÚdirec© r úš/home/alex/Desktop/Github/path-planning-algorithms/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)ÚblocksZAABBÚ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 s¤ t jdddgdddgdddgdddgdddgdddgdddgdddggdd�}t ddddgddddgdd d
|
|
|
|
|
|
dgd ddd
|
|
|
|
|
|
gdddd gdddd
|
|
|
|
|
|
gg¡}|jd }|d d …d d…f |d d …d d …f }t d | d f¡}t jd| dfd
|
|
|
|
|
|
d�}xpt|ƒD ]d}||| ||d d …f ||d |d d …d d …f< ||d ||d |d d …d d …f< qøW t| ƒtk�r~| || ¡ n"t|| dddd�} | | ¡}
|
|
|
|
|
|
|
|
|
|
|
|
|
S d S )zI
|
2020-06-24 13:58:07 -07:00
|
|
|
|
Subroutine used by draw_map() to display the environment blocks
|
|
|
|
|
|
r r Úfloat)Údtypeé é r r r é Né Úint64g333333Ã?Úk)ÚalphaZ
|
|
|
|
|
|
linewidthsZ
|
|
|
|
|
|
edgecolors) r r ÚshapeÚzerosÚrangeÚtyper Z set_vertsZadd_collection3d) Úaxr ÚvÚfÚnÚdZvlZflr ZpcÚhr r r
|
|
|
|
|
|
Údraw_block_list s FF
|
2020-06-23 19:37:10 -07:00
|
|
|
|
(8,
|
|
|
|
|
|
r, c C s` t j | jjdd… | jjdd… ¡}t j ¡ }t| |ƒrBt| ƒS |dk rR| jjS t |¡S dS )zbiased samplingr r r gš™™™™™©?N) r ÚrandomÚuniformÚenvÚboundaryÚisinsideÚ
|
|
|
|
|
|
sampleFreeÚgoalr )Ú
|
|
|
|
|
|
initparamsr r r r r
|
|
|
|
|
|
r2 B s &
|
|
|
|
|
|
|
2020-06-24 13:58:07 -07:00
|
|
|
|
r2 c C s† x€| j jD ]t}|d |d kr.|d k r
|
|
|
|
|
|
n q
|
2020-06-23 19:37:10 -07:00
|
|
|
|
|d |d krR|d k r
|
|
|
|
|
|
n q
|
2020-06-24 13:58:07 -07:00
|
|
|
|
|d |d krv|d k r
|
2020-06-23 19:37:10 -07:00
|
|
|
|
n q
|
2020-06-24 13:58:07 -07:00
|
|
|
|
dS q
|
|
|
|
|
|
W dS ) zsee if inside obstacler r r r r r TF)r/ r )r4 r r r r r
|
|
|
|
|
|
r1 N s lr1 c C sZ t ||ƒ}t||ƒ}x@t| jjƒD ]0}tj ||¡}|dk r"t||ƒ}||kr"dS q"W dS )z see if line intersects obstacleNTF)r r r r/ r ÚpyrrÚgeometric_testsÚray_intersect_aabb)r4 r r
|
2020-06-23 19:37:10 -07:00
|
|
|
|
ÚrayÚdistr ZshotZ dist_wallr r r
|
2020-06-24 13:58:07 -07:00
|
|
|
|
Ú isCollideV s
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
r: c C sT t | j¡}| jdkr | jd S t|t|ƒdƒ}t jj|| dd�}| jt |¡ S )Nr r )Úaxis) r r ÚVr r ÚlenÚlinalgÚnormÚargmin)r4 r r< ÚxrZdistsr r r
|
|
|
|
|
|
Únearestc s
|
|
|
|
|
|
|
|
|
|
|
|
rB c C s* || t j || ¡ }|| j| }|S )N)r r> r? Ústepsize)r4 r r
|
|
|
|
|
|
r Úxnewr r r
|
2020-06-23 19:37:10 -07:00
|
|
|
|
Ústeerl s rE c C s` | j }t | j¡}| jdkr&| jd S t|t|ƒdƒ}tjj|| dd�|k }|| }t |¡S )Nr r )r; ) rC r r r< r r r= r> r? )r4 r Úrr< rA ZinsideZ
|
2020-06-24 13:58:07 -07:00
|
|
|
|
nearpointsr r r
|
2020-06-23 19:37:10 -07:00
|
|
|
|
Únearr s
|
|
|
|
|
|
|
|
|
|
|
|
rG c C sR t || jjkƒrdS | jt|d ƒ t|d ƒ t|d ƒ }t| |ƒt||ƒ S )z-here use the additive recursive cost functionr r r )Úallr/ ÚstartÚParentÚstrÚcostr )r4 r Zxparentr r r
|
|
|
|
|
|
rL ~ s *rL c
|
|
|
|
|
|
C sv t | j¡}| j}t | j¡}| jj}| jj}tj ddd�}|j
|
2020-06-24 13:58:07 -07:00
|
|
|
|
ddd� | ¡ t || jj
|
2020-06-23 19:37:10 -07:00
|
|
|
|
ƒ | ¡ }|g kràxn|D ]f}|d d |d d f} |d d |d d f}
|
2020-06-24 13:58:07 -07:00
|
|
|
|
|d d |d d f} tj | |
|
|
|
|
|
|
| ¡} | | ¡ qvW |g k�r^xr|D ]j}|d d |d d f} |d d |d d f}
|
|
|
|
|
|
|d d |d d f} tjj| |
|
|
|
|
|
|
| d
|
|
|
|
|
|
d �} | | ¡ qðW |j|dd… |dd … |d d … d d
|
|
|
|
|
|
dd� |j|dd… |dd … |d d … dd
|
|
|
|
|
|
dd� | |d d …df |d d …df |d d …d f ¡ t | jjd | jjd ¡ t | jjd | jjd ¡ | | jjd | jjd ¡ t d¡ t d¡ |g k�sjt d¡ nt ¡ d S )Néo Z3d)Z
|
|
|
|
|
|
projectiong éZ )ZelevZazimr r r rF )ZcolorZgor r )Z
|
|
|
|
|
|
markersizeZmarkeredgecolorZror r r r r
|
|
|
|
|
|
gü©ñÒMbP?) r r r< ÚEÚPathr/ rI r3 ÚpltZsubplotZ view_initÚclearr, r Úget_edgeÚplt3dZart3dZLine3DZadd_lineZplotZ scatter3DZxlimr0 ZylimZset_zlimZxlabelZylabelÚpauseÚshow)
|
|
|
|
|
|
r4 r< rO rP rI r3 r& Úedgesr ÚxsZysÚzsÚliner r r
|
|
|
|
|
|
Ú
|
