
    ni                     .    S SK Jr  S SKr " S S\5      rg)    )bullet_clientNc                   2    \ rS rSrS rS rS rS rS rSr	g)	QuadrupedPoseInterpolator   c                     g )N )selfs    p/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py__init__"QuadrupedPoseInterpolator.__init__   s        c                 X    US   US   -
  U-  US   US   -
  U-  US   US   -
  U-  /nU$ Nr         r   )r	   posStartposEnd	deltaTimevels        r
   ComputeLinVel'QuadrupedPoseInterpolator.ComputeLinVel
   sL    1I#y06!9x{3Ji2W1I#y02CJr   c                     UR                   " X5      nUR                  " U5      u  pgUS   U-  U-  US   U-  U-  US   U-  U-  /nU$ r   )getDifferenceQuaterniongetAxisAngleFromQuaternion)	r	   ornStartornEndr   r   dornaxisangleangVels	            r
   ComputeAngVel'QuadrupedPoseInterpolator.ComputeAngVel   s_    00BD::4@KDAw)+d1go-JAw)+-FMr   c                     US   * US   * US   * US   /nUR                   " / SQU/ SQU5      u  pgUR                  " U5      u  pUS   U	-  U-  US   U	-  U-  US   U	-  U-  /n
U
$ )Nr   r   r      )r   r   r   )multiplyTransformsr   )r	   r   r   r   r   ornStartConjugatepos_diffq_diffr   r   r    s              r
   ComputeAngVelRel*QuadrupedPoseInterpolator.ComputeAngVelRel   s    "1+|hqk\8A;O$77	CTV_8>@H::6BKDAw)+d1go-JAw)+-FMr   c                    US   nUS   US   US   /nUS   US   US   /nUS   XS   US   -
  -  -   US   XS   US   -
  -  -   US   XS   US   -
  -  -   /U l         U R                  XgU5      U l        US   US   US   US   /nUS   US   US   US   /n	UR                  " XU5      U l        U R                  XUU5      U l        U R                   S   U R                   S   U R                   S   U R                  S   U R                  S   U R                  S   U R                  S   /n
U R                  S   U R                  S   U R                  S   U R                  S   U R                  S   U R                  S   /n[        S	5       HB  nUS
-   nX-   nX=   nXX-
  -  -   nX-
  U-  nU
R                  U5        UR                  U5        MD     Xl	        Xl
        X4$ )Nr   r   r   r$   r                  )_basePosr   _baseLinVelgetQuaternionSlerp_baseOrnr!   _baseAngVelrangeappend_jointPositions_jointVelocities)r	   frameFraction	frameDataframeDataNextr   keyFrameDurationbasePos1StartbasePos1EndbaseOrn1StartbaseOrn1NextjointPositionsjointVelocitiesjindexjointPosStartjointPosEndjointPosjointVels                     r
   SlerpQuadrupedPoseInterpolator.Slerp   s[    |q\9Q<1>M #]1%5}Q7GHKa=N]1=M,MNNa=N]1=M,MNNa=N]1=M,MNNDM
 ))-FVWDq\9Q<1y|LM!!$mA&6a8H-XYJZ[L!44]R_`DM))-GW*79D 	a$--*DMM!,<dmmA>NPTP]P]^_P`a$--*N
 	T--a0$2B2B12EtGWGWXYGZT--a0O
 2Y!ee&m!(k+2M!NNh-1AAhH%X&  *+**r   )r5   r2   r4   r1   r8   r9   N)
__name__
__module____qualname____firstlineno__r   r   r!   r)   rJ   __static_attributes__r   r   r
   r   r      s    	
#+r   r   )pybullet_utilsr   mathobjectr   r   r   r
   <module>rT      s    ( =+ =+r   