
    niv                     8    S SK r " S S\5      r " S S\5      rg)    Nc                   2    \ rS rSrS 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_utils/pd_controller_stable.py__init__#PDControllerStableMultiDof.__init__       H    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$ )Nr         )getDifferenceQuaterniongetAxisAngleFromQuaternion)	r   ornStartornEnd	deltaTimebullet_clientdornaxisangleangVels	            r   computeAngVel(PDControllerStableMultiDof.computeAngVel	   s_    00BD::4@KDAw)+d1go-JAw)+-FMr   c                 v   US   US   -  US   US   -  -   US   US   -  -   US   US   -  -
  US   US   -  US   US   -  -   US   US   -  -   US   US   -  -
  US   US   -  US   US   -  -   US   US   -  -   US   US   -  -
  US   US   -  US   US   -  -
  US   US   -  -
  US   US   -  -
  /$ )N   r   r   r    )r   q1q2s      r   quatMul"PDControllerStableMultiDof.quatMul   s   
1111%11511E
1111%11511E
1111%11511E
1111%11511E	 r   c                     US   * US   * US   * US   /nU R                  UU5      nUR                  U5      u  pxUS   U-  U-  US   U-  U-  US   U-  U-  /n	U	$ )Nr   r   r   r"   )r&   r   )
r   r   r   r   r   ornStartConjugateq_diffr   r   r   s
             r   computeAngVelRel+PDControllerStableMultiDof.computeAngVelRel   s    "1+|hqk\8A;O\\F  ::6BKDAw)+d1go-JAw)+-FMr   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US   US   US   US   /nU R                  R	                  UU5      n/ SQnUS   U
S   -
  US   U
S   -
  US   U
S   -
  US   US   US   S/n[
        R                  " U5      nS	nS	n/ S
QnSnU(       a  U R                  R                  X5      n[        U	5       GHa  nU(       a  WU   nO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                  UUSU R                  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Md     [
        R                  " U5      n"[
        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 R                  R                  XSS9n+[
        R                  " U+5      n,U,U(U-  -   n-U R                  R                  XUUSS9n.[
        R                  " U.5      n/U-n0U)U*-   U/-
  n1Sn2U2(       a!  [
        R                   R#                  U0U15      n3O)U R                  R%                  XU1R'                  5       XhS9n3U)U*-   U(R                  U35      U-  -
  n4[
        R                  " U5      n5[
        R(                  " U4U5* U55      n6U6$ )Nr   r   r   r"   r         r   r   r      )r   r   r   r   r   r   r   T        )r2   r2   r2   r2   )flags)jointPositionsbkdt)lenr	   getBasePositionAndOrientationgetBaseVelocitygetAxisDifferenceQuaternionnparraygetJointStatesMultiDofrangegetJointStateMultiDofappendr+   diagflatdotcalculateMassMatrixcalculateInverseDynamicslinalgsolve	ldltSolvetolistclip)7r   bodyUniqueIdjointIndicesdesiredPositionsdesiredVelocitieskpskds	maxForcestimeStep	numJointscurPoscurOrnr$   
baseLinVel
baseAngVelqdot1
desiredOrnaxis1angDiffqError
target_posqIndex	qdotIndexzeroAccelerationsuseArrayjointStatesijsjointPosjointVel
desiredPosqdiffjointVelNew
desiredVelqqerrqdotqdotdesiredqdoterrKpKdpdM1M2Mc1cAr5   useNumpySolverqddottaumaxFforcess7                                                          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
 	-a02B12EGWXYGZJ HH00VDEGfQi')9!)<vay)HfQi'WQZQF *+J FI-HHH33LNk9	^XX++Lq/JAhAhHnb	RUq%f-
Xa[(e  $!Q		RUqV$&6vz&BDTU[^_U_D`VaZ(


 ''*aJ  {HQK!a@gaj!gaj!gaj!ai(*;IM*Ji!m,

 	--!Q	Q T 	A88FD 88E?D ((,-KD G	S	B	S	B
vA
 	wA
 
	%	%la	%	@B	"B 
b8m	A 
	*	*<UDU]^	*	_B
A	A
 	
A	A Niiooa#ehh  AHHJSV ce
a%"&&-(*
*C 88IDWWS4%&F Mr   r   N)
__name__
__module____qualname____firstlineno__r   r   r&   r+   r   __static_attributes__r#   r   r   r   r      s    	Kr   r   c                        \ rS rSrS rS rSrg)PDControllerStable   c                     Xl         g r   r   r
   s     r   r   PDControllerStable.__init__   r   r   c	                    Sn	Sn
U R                   R                  US5      S   nU R                   R                  U5      u  p/ n/ n/ n/ nUS:  a^  Sn	Sn
US   US   US   US   US   US   US   /nS/U	-  nS/U	-  n/ SQnUS   US   -
  US   US   -
  US   US   -
  US   US   US   /nU R                   R                  U5      nU R                   R	                  X5      n[        U5       HB  nUR                  UU   S   5        UR                  UU   S   5        UR                  S5        MD     [        R                  " U5      n[        R                  " U5      n[        R                  " U5      n[        R                  " U5      n[        U5       H#  nUR                  UUU
-      UUU
-      -
  5        M%     UU-
  n[        R                  " U5      n[        R                  " U5      nUR                  U5      nUR                  U5      nUU-   n U R                   R                  X5      n![        R                  " U!5      n"U"UU-  -   n#U R                   R                  XUU5      n$[        R                  " U$5      n%U#n&U%* U-   U-   n'[        R                  R                  U&U'5      n(UU-   UR                  U(5      U-  -
  n)[        R                  " U5      n*[        R                  " U)U** U*5      n)U)$ )	Nr   r/   r1   r   r   r"   r0   )r	   getDynamicsInfor9   getNumJointsgetJointStatesr?   rA   r<   r=   rB   rC   rD   rE   rF   rG   rJ   )+r   rK   rL   rM   rN   rO   rP   rQ   rR   numBaseDofsnumPosBaseDofsbaseMassrT   rU   r$   rX   r`   r\   r[   rS   rb   rc   rk   rm   qdesqdotdesj	qdotErrorrp   rq   rr   rs   r~   rt   ru   rv   rw   rx   ry   r5   r{   r|   r}   s+                                              r   r   PDControllerStable.computePD   s   KNxx''b9!<HXX;;LINF	BEF1kn1Ivay&)VAYq	6!9fUViXbcKe#+g
1
q	
)+;A+>+J
1
q	
)71:wqz71:f %%l3I(()),EK9iiAq!"ll;q>!$%q!  	A88E?D88$%Dhh()G9mm$Q%782a.>P;QQR  $I	S	B	S	B
vA
yAUF		%	%l	7B	"B	b8m	A		*	*<UDU	VB
A	A	
Q
AIIOOAq!E
a%"&&-(*
*C88ID
''#ud
#CJr   r   N)r   r   r   r   r   r   r   r#   r   r   r   r      s    7r   r   )numpyr<   objectr   r   r#   r   r   <module>r      s#    j jZ< <r   