
    ni7                     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)PDControllerStableMultiDof   c                     Xl         g N_pbselfpbs     ^/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/pdControllerStable.py__init__#PDControllerStableMultiDof.__init__       H    c	                    [        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UU-  -
  5      n#U"R                  U 5      n$U R                  R                  XSS
9n%[        R                  " U%5      n%U R                  R                  XUUSS
9n&[        R                  " U&5      n&[        R                  R                  U%U"U-  -   U#U$-   U&-
  S9n'U#U$-   U"R                  U'5      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   )flagsab)lenr	   getBasePositionAndOrientationgetBaseVelocityrangegetJointStateMultiDofappendgetAxisDifferenceQuaternionnparraydiagflatdotcalculateMassMatrixcalculateInverseDynamicslinalgsolve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_termd_termMGqddottaumaxFgeneralized_forcess+                                              r   	computePD$PDControllerStableMultiDof.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 VVFT(]*+FVVG_F 	$$\Q$?A
A 	)),ECT\])^A
AIIOOq2=0$vo1  3E 6/BFF5MH4
4C88IDteT2r   r   N)__name__
__module____qualname____firstlineno__r   rV   __static_attributes__ r   r   r   r      s    Tr   r   c                   $    \ rS rSrSrS rS rSrg)PDControllerStable`   z
Implementation based on: Tan, J., Liu, K., & Turk, G. (2011). "Stable proportional-derivative controllers"
DOI: 10.1109/MCG.2011.30
c                     Xl         g r   r   r
   s     r   r   PDControllerStable.__init__e   r   r   c	                 6   U R                   R                  U5      n	U R                   R                  X5      n
/ n/ n/ n[        U	5       H@  nUR	                  X   S   5        UR	                  X   S   5        UR	                  S5        MB     [
        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UU-  -
  5      nUR                  U5      nU R                   R                  X5      n[
        R                  " U5      nU R                   R                  XX5      n[
        R                  " U5      n[
        R                  R                  UUU-  -   U* U-   U-   S9nUU-   UR                  U5      U-  -
  n[
        R                  " U5      n[
        R                  " UU* U5      nU$ )Nr   r   r   )r	   getNumJointsgetJointStatesr   r!   r#   r$   r%   r&   r'   r(   r)   r*   r+   )r   r,   r-   r.   r/   r0   r1   r2   r3   r4   jointStatesr7   r:   r>   r?   rH   rI   qdesqdotdesr;   	qdotErrorrL   rM   rN   rO   rP   rQ   rR   rS   rT   rU   s                                  r   rV   PDControllerStable.computePDh   s   %%l3I(()),EK	BE9iiq!"ll;>!$%q! 
 	A88E?D88$%Dhh()GAXF$I	S	B	S	B VVFT(]*+FVVIF 	$$\6A
A 	)),EUA
AIIOOq2=0 !rF{V3  6E 6/RVVE]X5
6C88IDteT2r   r   N)rX   rY   rZ   r[   __doc__r   rV   r\   r]   r   r   r_   r_   `   s    +r   r_   )numpyr#   objectr   r_   r]   r   r   <module>rn      s#    Y Yx3 3r   