
    ni(                     8    S SK r " S S\5      r " S S\5      rg)    Nc                        \ rS rSrS rS rSrg)PDControllerExplicitMultiDof   c                     Xl         g N_pbselfpbs     `/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/pdControllerExplicit.py__init__%PDControllerExplicitMultiDof.__init__       H    c	                 p   [        U5      n	U R                  R                  U5      u  pU
S   U
S   U
S   US   US   US   US   /nU R                  R                  U5      u  pUS   US   US   US   US   US   S/n/ SQnSnSn/ SQn[	        U	5       GHR  nU R                  R                  XU   5      nUS   nUS   nUU-  n[        US   5      S:X  a>  UU   nUUS   -
  nUR                  U5        UR                  S5        UU-  nUS-  nUS-  n[        US   5      S:X  d  M  UU   UUS-      UUS-      UUS-      /nU R                  R                  UU5      nUS   US   US   S/nUU-  nUR                  US   5        UR                  US   5        UR                  US   5        UR                  S5        UU   UUS-      UUS-      /nU/ S	Q-  nUS-  nUS-  nGMU     [        R                  " U5      n[        R                  " U5      n[        R                  " U5      nUU-
  n [        R                  " U5      n![        R                  " U5      n"U!R                  U5      n#U"R                  U 5      n$U#U$-   n%[        R                  " U5      n&[        R                  " U%U&* U&5      n%U%$ )
Nr            )r   r   r   r   r   r   r              r   )r   r   r   r   )lenr	   getBasePositionAndOrientationgetBaseVelocityrangegetJointStateMultiDofappendgetAxisDifferenceQuaternionnparraydiagflatdotclip)'r   bodyUniqueIdjointIndicesdesiredPositionsdesiredVelocitieskpskds	maxForcestimeStep	numJointscurPoscurOrnq1
baseLinVel
baseAngVelqdot1qErrorqIndex	qdotIndexzeroAccelerationsijsjointPosjointVel
desiredPosqdiffaxisjointVelNew
desiredVelqqdotqdotdesiredqdoterrKpKdpdforcesmaxFs'                                          r   	computePD&PDControllerExplicitMultiDof.computePD	   s    L!IXX;;LINF
)VAYq	6!9fQiFSTI	VB!XX55lCJ1z!}jmZ]JqMS]^_S`bcE #FFI-988)),QHbAhAhHnb	RUq%f-
Xa[(e  $!Q		RUqV$&6vz&BDTU[^_U_D`VaZ(

 xx33JI{HQK!a@d1gd1gd1gai(*;IM*Ji!m,

 	--!Q	E H 	A88E?D((,-KD G	S	B	S	B
vA
wAUF88IDWWVdUD)FMr   r   N__name__
__module____qualname____firstlineno__r   rJ   __static_attributes__ r   r   r   r      s    =r   r   c                        \ rS rSrS rS rSrg)PDControllerExplicitI   c                     Xl         g r   r   r
   s     r   r   PDControllerExplicit.__init__K   r   r   c	                    U R                   R                  U5      n	U R                   R                  X5      n
/ n/ n[        U	5       H/  nUR	                  X   S   5        UR	                  X   S   5        M1     [
        R                  " U5      n[
        R                  " U5      n[
        R                  " U5      n[
        R                  " U5      nUU-
  nUU-
  n[
        R                  " U5      n[
        R                  " U5      nUR                  U5      UR                  U5      -   n[
        R                  " U5      n[
        R                  " UU* U5      nU$ )Nr   r   )
r	   getNumJointsgetJointStatesr   r   r   r    r!   r"   r#   )r   r$   r%   r&   r'   r(   r)   r*   r+   r,   jointStatesr/   r2   r7   r@   rA   qdesqdotdesr3   	qdotErrorrD   rE   rH   rI   s                           r   rJ   PDControllerExplicit.computePDN   s   %%l3I(()),EK	BE9iiq!"ll;>!$%  	A88E?D88$%Dhh()GAXF$I	S	B	S	BVVF^bffY//F88IDWWVdUD)FMr   r   NrL   rR   r   r   rT   rT   I   s    r   rT   )numpyr   objectr   rT   rR   r   r   <module>rb      s#    B6 BJ6 r   