o *jiF@sddlmZddlmZmZmZmZmZmZddl Z ddl Z e j e ddfd>ddZe jde jdZgdgdgdged<gdgdgdged<gdgdgdged<gdgdgdged<gdgdgd ged!<gd"gd gdged#<gd gd"gdged$<gd gd%gdged&<gd"gd gd'ged(<gd)gdgd ged*<ed+d+d,Ze eae d-Zgd.gd/gd0gd1gedddddf<gd2gd.gd3gd0geddddd4f<gd5gd1gd.gd2geddddd6f<gd3gd5gd/gd.geddddd7f<eddd4dddfZe eaGd8d9d9ZGd:d;d;ZGdZd`dAdBZedadEdFZdYdGdHZdbdIdJZdcdKdLZdYdMdNZdOS)dRotationmatrcCs,|jdddkrtd|j||_dS)Nr!r zincorrect rotation shape: )shape ValueError_mat)selfr$rrr__init__>s zRotation.__init__r Fr r rrrrrcCsRtjd|||d}|jgdt|ddR}|jg|ddR}t|S)Nrrrr)reyeviewlenexpandr#)r%r rrr$rrridentityFs  zRotation.identityabcCs|||jSN)floattyper )r0r1rrr mat_mul_matSszRotation.mat_mul_matrtcCs$||dd|jSNr)r3 unsqueezeZsqueezer4r )r6r7rrr mat_mul_vecWs$zRotation.mat_mul_vecindexrcCs0t|ts|f}t|j|tdtdfdSNr$) isinstancetupler#r'slicer(r;rrr __getitem__[s  zRotation.__getitem__rightcCsRt|ttfrt|j|dSt|tjrt|j|ddStdt|d)Nr=).NNz/multiplicand must be a tensor or a number, got .) r>intr3r#r'rTensor TypeErrorr4)r(rCrrr__mul__`s zRotation.__mul__leftcC ||Sr2rHr(rIrrr__rmul__js zRotation.__rmul__othercCst|j|j}t|dSr<)r#r5rot_mat)r(rNZnew_matrrr __matmul__ms zRotation.__matmul__cCs|jddS)Nrr!)r' transposer(rrr_inv_matqszRotation._inv_matcC|jSr2)r'rRrrrrOuszRotation.rot_matcCs t|jdSr<)r#rSrRrrrinvertys zRotation.invertptscCt|j|Sr2)r#r:r'r(rVrrrapply|zRotation.applycCrWr2)r#r:rSrXrrr invert_applyrZzRotation.invert_apply torch.SizecC|jjdd}|S)Nr!)r'r%r(srrrr%zRotation.shape torch.dtypecC|jjSr2)r'r rRrrrr zRotation.dtype torch.devicecCrbr2)r'rrRrrrrrczRotation.devicecCrbr2)r'rrRrrrrrczRotation.requires_graddimrEcCs<|t|jkr td|j|dkr|n|d}t|dS)NInvalid dimensionrrr=)r-r%r&r'r9r#)r(rerot_matsrrrr9s zRotation.unsqueezefn&Callable[[torch.Tensor], torch.Tensor]cCs`|j|jjddd}tjtt|tj|dddd}||jddd}t|dS)Nr!)r"rrer r=) r'r,r%rstacklistmapunbindr#)r(rhr$rrr map_tensor_fns" zRotation.map_tensor_fnrsSequence[Rotation]cCs6dd|D}tj||dkr|n|dd}t|dS)NcSg|]}|jqSr)rO).0r6rrr z Rotation.cat..rrrjr=)rcatr#)rprergrrrrvs z Rotation.catcCt|jdSr<)r#r'cudarRrrrrxz Rotation.cudacCst|jj||ddS)N)rr r=)r#r'to)r(rr rrrrzsz Rotation.tocCst|j|dSr<)r#r'r4r(r rrrr4z Rotation.typecCrwr<)r#r'detachrRrrrr}ryzRotation.detachN)r$r)r r rrrrrr#)r0rr1rrr)r6rr7rrr)r;rrr#)rCrrr#)rIrrr#)rNr#rr#rrrr#rVrrrrr\rrarrd)rr)rerErr#)rhrirr#)rprqrerErr#)rrr r rr#)r r rr#)__name__ __module__ __qualname__r) staticmethodrr3rr/r5r:rBrHrMrPpropertyrSrOrUrYr[r%r rr9rorvrxrzr4r}rrrrr#<sL                     r#c@sNeZdZdbddZeejeddfdcddZddddZ deddZ dfddZ e dgd d!Z e dhd#d$Zdid&d'Zdjd(d)Zdkd+d,Zdld.d/Zdld0d1Zdmd2d3Zdnd6d7Zdjd8d9Zedod;d<Ze =dpdqdCdDZdrdGdHZedsdKdLZdtdNdOZdndPdQZdudSdTZdmdUdVZedvdXdYZdmdZd[Ze dwd]d^Z dmd_d`Z!daS)xFramerotationOptional[Rotation] translationOptional[torch.Tensor]cCs|dur|durtd}td}n$|dur"t|j|j|j|j}n|dur6t|jdd|j|j|j}|j|jddksF|j|jkrJtd||_||_ dS)N)rrz+RotationMatrix and translation incompatible) r#r/rr%r rrr&_r_t)r(rrrrrr)s*     zFrame.__init__r Fr% Iterable[int]r r rrrrrcCs tt||||t||||Sr2)rr#r/r)r%r rrrrrr/s zFrame.identityr;rcCs2t|tkr |f}t|j||j|tdfSr2)r4r?rrrr@rArrrrBs zFrame.__getitem__rCrcCs6t|tjs td|j|}|j|d}t||S)Nz'The other multiplicand must be a Tensor).N)r>rrFrGrrr)r(rCnew_rots new_transrrrrHs   z Frame.__mul__rIcCrJr2rKrLrrrrMs zFrame.__rmul__r\cCr]r8)rr%r^rrrr%r`z Frame.shaperdcCrbr2)rrrRrrrrrcz Frame.devicer#cCrTr2rrRrrrget_rots zFrame.get_rotscCrTr2rrRrrr get_trans rzFrame.get_transrNcCs*|j|j}|j|j|j}t||Sr2)rrYrr)r(rNZnew_rotrrrrcomposes  z Frame.composerVcCs|j|}||jSr2)rrYr)r(rVZrotatedrrrrYs  z Frame.applycCs||j}|j|Sr2)rrr[rXrrrr[s  zFrame.invert_applycCs$|j}||j}t|d|Sr8)rrUrYrr)r(Zrot_invZtrn_invrrrrU"s  z Frame.invertrhricCs:|j|}tjtt|tj|jdddd}t||S)Nrrj) rrorrkrlrmrnrr)r(rhrrrrrro(s  zFrame.map_tensor_fncCsV|jg|jddR}|jj|dddddf<|j|ddddf<d|d<|S)Nr.rr).rr)r new_zerosr%rrO)r(Ztensorrrr to_tensor_4x40s zFrame.to_tensor_4x4r7cCsR|jdddkr tdt|dddddfd}|ddddf}t||S)Nr!)rrzIncorrectly shaped input tensor.rr=)r%r&r#r)r7rotsrrrrfrom_tensor_4x47s  zFrame.from_tensor_4x4:0yE> p_neg_x_axisorigin p_xy_planeepsr3c stj|dd}tj|dd}tj|dd}ddt||D}ddt||D}ttdd|D|fdd|D}td dt||Dfd dt||D}ttd d|D|fd d|D}|d |d|d|d |d|d|d|d|d|d |d |dg}tjddt|||Ddd}||jddd}t|d}t |tj|ddS)NrrjcSg|]\}}||qSrrrsc1c2rrrrtLz'Frame.from_3_points..cSrrrrrrrrtMrcs|]}||VqdSr2rrscrrr Oz&Frame.from_3_points..cg|]}|qSrrrdenomrrrtPcss|] \}}||VqdSr2rrrrrrQscsg|] \}}||qSrrr)dotrrrtRscsrr2rrrrrrSrcrrrrrrrrtTrrrrcSsg|] }|D]}|qqSrr)rstuprrrrrt[sr r=) rrnzipsqrtsumrkreshaper%r#r) rrrrZe0e1e2rrot_objr)rrr from_3_pointsAs&  zFrame.from_3_pointsrerEcCsH|t|jkr td|j|}|j|dkr|n|d}t||S)Nrfrr)r-r%r&rr9rr)r(rerrrrrr9bs   zFrame.unsqueezeTsSequence[Frame]cCsHtdd|D|}tjdd|D|dkr|n|dd}t||S)NcSrrrrrsTrrrrtrruzFrame.cat..cSrrrrrrrrrtsrurrrj)r#rvrr)rrerrrrrrvms( z Frame.catCallable[[Rotation], Rotation]cCst||j|jSr2rrrr(rhrrr apply_rot_fnwr|zFrame.apply_rot_fncCst|j||jSr2rrrrrapply_trans_fnzszFrame.apply_trans_fntrans_scale_factorcsfdd}||S)Ncs|Sr2r)r7rrrrhz#Frame.scale_translation..fn)r)r(rrhrrrscale_translation~s  zFrame.scale_translationcCsdd}||S)NcSs|Sr2)r})r6rrrrhrz#Frame.stop_rot_gradient..fn)rrrrrstop_rot_gradients zFrame.stop_rot_gradient#B ;cs$|j}|}||fddtdD\}}}t||d|d}| |} ||} | g| jddR} | | d<d| | d<| | d<| | d <d | d <t||d|d|d}||} t|d|d|} | g| jddR}| |d<| |d <d |d <d| |d <| |d <t|| }t |fddtdD\}}}t||d|d}| |}||}| g| jddR}d |d<||d <d||d<||d<||d <t||}| dd}t| |d}t || |S)Ncg|]}d|fqS.rrsi)c_xyzrrrtrz7Frame.make_transform_from_reference..rr).rrr).rr).rr).rrr).rr).rr).rrcrrrr)n_xyzrrrtr).rr).rrr!r=) r r3rangerrrr%r#r5r:rQr4r)rZca_xyzrrZ input_dtypeZc_xZc_yZd_pairnormZsin_c1Zcos_c1Zc1_rotsZsin_c2Zcos_c2Zc2_rotsZc_rots_Zn_yZn_zZsin_nZcos_nZn_rotsrrr)rrrmake_transform_from_referencesR  "       z#Frame.make_transform_from_referencecCst|j|jSr2)rrrxrrRrrrrxsz Frame.cudaracCs|jj|jjks J|jjSr2)rr rrRrrrr sz Frame.dtypecCst|j||j|Sr2)rrr4rr{rrrr4sz Frame.typeN)rrrr) r%rr r rrrrrr)r;rrr)rCrrr)rIrrrrrrr~)rNrrrr)rr)rhrirr)r7rrr)r) rrrrrrrr3rr)rerErr)rrrerErr)rhrrr)rr3rr)rr)"rrrr)rrr3rr/rBrHrMrr%rrrrYr[rUrorrrr9rvrrrrrrxr r4rrrrrsP                     5 rc@seZdZd,ddZeejeddfd-ddZddZ ddZ ddZ eddZ eddZ eddZ d.d/d"d#Zd0d'd(Zd1d)d*Zd+S)2 Quaternion quaternionrrcCs.|jddkrtd|j||_||_dS)Nrrzincorrect quaternion shape: )r%r&_qr)r(rrrrrr)s zQuaternion.__init__r Fr%rr r rrrrrcCsdt||||}tjg|dR|||d}t d|d<Wdn1s(wYt||S)Nrrr).r)rrrZno_gradr)r%r rrrquatsrrrr/s   zQuaternion.identitycCrTr2)rrRrrr get_quatsrzQuaternion.get_quatscCrTr2rrRrrrrrzQuaternion.get_transcCs|}t|}|Sr2)rr quat_to_rot)r(rrgrrr get_rot_matss zQuaternion.get_rot_matscCs|j}|}tj|jkrt|jatjt|dddddf|dddddfdd}||}|jg|j ddddR}|S)N.r!rjrr) r r3_QUAT_TO_ROT_tensorrrzrrr4r,r%)Znormalized_quatr Z rot_tensorrrrrs  "zQuaternion.quat_to_rotcCs2|j}|}|tjj|ddd}||}|S)NrT)reZkeepdim)r r3rZlinalgrr4)rr rrrnormalize_quats  zQuaternion.normalize_quatcCs|j}|}|}tj|jkrt|jat}|dt|jdd|j}tj ||dddddf|dddddfdd |S)Nr*r.rrj) r r3_QUAT_MULTIPLY_BY_VEC_tensorrrzr,r-r%rrr4)ZquatZvecr r$Z reshaped_matrrrquat_multiply_by_vec s "*zQuaternion.quat_multiply_by_vecT q_update_vecnormalize_quatscCs*|}|t||}|rt|}|Sr2)rrrr)r(rrr new_quatsrrrcompose_q_update_vecs zQuaternion.compose_q_update_vec update_vec pre_rot_matr#cCsJ|dddf|dddf}}||}||}|j|}t||S)N.r)rrYrr)r(rrZq_vecZt_vecrZ trans_updaterrrrcompose_update_vec$s "    zQuaternion.compose_update_veccCst|j|jSr2)rrr}rrRrrrr1r|zQuaternion.stop_rot_gradientN)rrrr) r%rr r rrrrrr)T)rrrrrr)rrrr#rr)rr)rrrr)rrr3rr/rrrrrrrrrrrrrrs(      r) r r r r rrrrrr) __future__rtypingrrrrrrnumpynprr3rrrZfloat32Z _QUAT_TO_ROTrZ from_numpyrZ_QUAT_MULTIPLYZ_QUAT_MULTIPLY_BY_VECrr#rrrrrrsR      {