
    ni
h                     F%   S SK rS SKrS SKrS SKrSr\(       a  \R                  " \R                  5        O\R                  " \R                  5        \R                  " \R                  " 5       5        Sr\(       + rSr\(       a  \R                  " \R                  S5        S SKJr  S SKJr  \" \5      r\" \5      r\R,                  " SSS	/ S
QS9  S SKr\R.                  " S5        SrSrSr\R                  " \R                  " 5       5        \R6                  " SS9  Sr\R6                  " \S9  \R                  " 5       S-   r\" S\5        \" \S5       r \RB                  " \ 5      r"SSS5        \" S\#" \"5      5        \" \"S   5        \#" \"S   5      r$\" S\$5        \RJ                  " SS \$S-
  S 5      r&\RJ                  " SS SS5      r'\RJ                  " SS SS5      r(\RJ                  " SS SS5      r)/ SQr*/ S Q/ S!Q/ S"Q/ S#Q/ S$Q/r+\RX                  " S%\+S    S    \+S    S   S-   \+S    S&   // S!Q5        \RX                  " S'\+S   S    \+S   S   S-   \+S   S&   // S!Q5        \RX                  " S(\+S&   S    \+S&   S   S-   \+S&   S&   // S!Q5        \RX                  " S)\+S*   S    \+S*   S   S-   \+S*   S&   // S!Q5        \RX                  " S+\+S,   S    \+S,   S   S-   \+S,   S&   // S!Q5        \RZ                  \R\                  -   r/\R`                  " S-\+S    S.S\/S/9r1\R`                  " S-\+S   S.S\/S/9r2\R`                  " S-\+S&   S.S\/S/9r3\R`                  " S-\+S*   S.S\/S/9r4\R`                  " S-\+S,   S.S\/S/9r5\Rl                  " \1S0S0S0\Rn                  / S!Q/ S!Q\+S    / S1Q5	      r8\Rl                  " \2S0S0S0\Rn                  / S!Q/ S!Q\+S   / S1Q5	      r9\Rl                  " \3S0S0S0\Rn                  / S!Q/ S!Q\+S&   / S1Q5	      r:\Rl                  " \4S0S0S0\Rn                  / S!Q/ S!Q\+S*   / S1Q5	      r:\Rl                  " \5S0S0S0\Rn                  / S!Q/ S!Q\+S,   / S1Q5	      r;/ S2Qr</ S3Qr=\R|                  " \1\+S    / S1Q5        \R|                  " \2\+S   / S1Q5        \R|                  " \3\+S&   / S1Q5        \R|                  " \4\+S*   / S1Q5        \R|                  " \5\+S,   / S1Q5        S4r?\@" \R                  " \15      5       GH2  rB\R                  " \1\B5      rDS /rE\DS&   rF\F\R                  :X  a  \<\?S-      \<\?S&-      \<\?S*-      \<\?S -      /rE\=\?S -      \=\?S-      \=\?S&-      /rH\?S,-  r?\" S5\E5        \" S6\H5        \R                  " \1\B\E\HS79  \R                  " \5\B\E\HS79  \R                  " \2\B\E\HS79  \F\R                  :X  d  \F\R                  :X  d  M  \<\?   /rE\=\?   /rH\?S-  r?\" S8\E5        \" S9\H5        \R                  " \1\B\E\HS79  \R                  " \5\B\E\HS79  \R                  " \2\B\E\HS79  GM5     \@" \R                  " \15      5       GH~  rB\R                  " \1\B5      rDS /rE\DS&   rF\F\R                  :X  a  / S1QrE\R                  " \1\B\R                  \E/ S!QS S/ S!QS:9  \R                  " \5\B\R                  \E/ S!QS S/ S!QS:9  \R                  " \3\B\R                  \E/ S!QS S/ S;QS:9  \R                  " \4\B\R                  \E/ S!QS S/ S<QS:9  \F\R                  :X  d  \F\R                  :X  ax  \R                  " \1\B\R                  S S S=9  \R                  " \3\B\R                  S S>S=9  \R                  " \4\B\R                  S S?S=9  \R                  " \5\B\R                  S S S=9  \" S@\BSA\*\DS&      5        \" S@\BSB\DS   5        GM     / rP/ rQ\@" \R                  " \15      5       Hp  rB\R                  " \1\B/ SCQSD9  \R                  " \1\B5      rS\(       a  M3  \SS   rT\SS&   rF\F\R                  :X  d  \F\R                  :X  d  M_  \PR                  \B5        Mr     \R                  " \1S&/ SEQSD9  SrVS&rWS*rXS,rYSFrZSGr[S4r\SHr]S?r^SIr_SJr`SKraS SKr/ SLQrb/ SMQrcSrd\R                  " SNSO5        \R                  " 5       (       G	a  \(       aI  \R                  " \'5      rh\R                  " \(5      ri\R                  " \)5      rj\R                  " \&5      rkOSrhSriSrjS rk\irl\m" \k5      rn\nS-   ro\o\$:  a  \nro\k\n-
  rp\"S   \n   rq\"S   \o   rr\qS   \qS&   \qS*   /rs\rS   \rS&   \rS*   /rt\sS    \p\tS    \sS    -
  -  -   \sS   \p\tS   \sS   -
  -  -   \sS&   \p\tS&   \sS&   -
  -  -   /ru\qSF   \qSG   \qS4   \qS,   /rv\rSF   \rSG   \rS4   \rS,   /rw\R                  " \v\w\p5      ry\(       a  / SPQrz\R                  " / SQQ5      r|\R                  " \z\|\u\y5      u  r~r\R|                  " \1\~\5        / SRQrz\R                  " / SQQ5      r|\R                  " \z\|\u\y5      u  r~r\R|                  " \2\~\5        \qSH   \qS?   \qSI   \qSS   /r\rSH   \rS?   \rSI   \rSS   /r\R                  " \\\p5      r\qSK   \qST   \qSU   \qSJ   /r\rSK   \rST   \rSU   \rSJ   /r\R                  " \\\p5      r\qSV   \qSW   \qSX   \qSY   /r\rSV   \rSW   \rSX   \rSY   /r\R                  " \\\p5      r\qSZ   /r\rSZ   /r\S    \p\S    \S    -
  -  -   /r\qS[   \qS\   \qS]   \qS^   /r\rS[   \rS\   \rS]   \rS^   /r\R                  " \\\p5      r\qS_   \qS`   \qSa   \qSb   /r\rS_   \rS`   \rSa   \rSb   /r\R                  " \\\p5      r\qSc   /r\rSc   /r\S    \p\S    \S    -
  -  -   /r\qS>   \qSd   \qSe   \qS   /r\rS>   \rSd   \rSe   \rS   /r\R                  " \\\p5      r\qSf   /r\rSf   /r\S    \p\S    \S    -
  -  -   /r\qSg   \qSh   \qSi   \qSj   /r\rSg   \rSh   \rSi   \rSj   /r\R                  " \\\p5      r\qSk   \qSl   \qSm   \qSn   /r\rSk   \rSl   \rSm   \rSn   /r\R                  " \\\p5      r\qSo   /r\rSo   /r\S    \p\S    \S    -
  -  -   /r Srd\GRH                  " S S Sp5        \irl\(       Ga  / SqQrS4r\ H  r\\-  rM
     \V\W\X\Y\Z\[\\\]\^\_\`\a/r\GRR                  " \15      u  r~r/ \~S    P\~S   P\~S&   P\S    P\S   P\S&   P\S*   P\S    P\S   P\S&   P\S*   P\S    P\S   P\S&   P\S*   P\S    P\S   P\S&   P\S*   P\S    P\S    P\S   P\S&   P\S*   P\S    P\S   P\S&   P\S*   P\S    P\S    P\S   P\S&   P\S*   P\S    P\S    P\S   P\S&   P\S*   P\S    P\S   P\S&   P\S*   P\S    Pr\GRW                  \5\\S /\-  \b\c\j/\-  \Sr9r\V\W\X\Y\Z\[\\\]\^\_\`\a/r\\\\\\\\\\\\/r\j\j\j/\j\j\j/\j\j\j/\j/\j\j\j/\j\j\j/\j/\j\j\j/\j/\j\j\j/\j\j\j/\j//rS/SJ-  rSs/SJ-  r\GRd                  " \1\\GRf                  \\\\St9  \GRW                  \3\\S /\-  \b\c\jSu-  /\-  \Sr9rS4r\@" \#" \5      5       H  r\\   r\\   S,:X  af  \R                  " \5\\GRp                  \\S -      \\S-      \\S&-      /Sv9  \R                  " \3\\GRp                  \\S -      \\S-      \\S&-      /Sv9  \\   S:X  aD  \R                  " \5\\GRp                  \\   /Sw9  \R                  " \3\\GRp                  \\   /Sw9  \\\   -  rM     \R                  " \2\V\R                  \\l\j/Sx9  \R                  " \2\W\R                  \\l\j/Sx9  \R                  " \2\X\R                  \\l\j/Sx9  \R                  " \2\Y\R                  \\l\j/Sx9  \R                  " \2\Z\R                  \\l\j/Sx9  \R                  " \2\[\R                  \\l\j/Sx9  \R                  " \2\\\R                  \\l\j/Sx9  \R                  " \2\]\R                  \\l\j/Sx9  \R                  " \2\^\R                  \\l\j/Sx9  \R                  " \2\_\R                  \\l\j/Sx9  \R                  " \2\`\R                  \\l\j/Sx9  \R                  " \2\a\R                  \\l\j/Sx9  Sr\(       a  \R                  " \4\V\5        \R                  " \4\W\5        \R                  " \4\X\5        \R                  " \4\Y\5        \R                  " \4\Z\5        \R                  " \4\[\5        \R                  " \4\\\5        \R                  " \4\]\5        \R                  " \4\^\5        \R                  " \4\_\5        \R                  " \4\`\5        \R                  " \4\a\5        \GRt                  " 5         \(       a]  \@" \R                  " \25      5       HA  rB\GRv                  " \2\B5      r\" Sy\BSz\S    5        \" Sy\BS{\S   5        \" Sy\BS|\S*   5        MC     \GRz                  " \5        \R                  " 5       (       a  G	M  gg! , (       d  f       GN,= f)}    NTF   )PDControllerExplicitMultiDof)PDControllerStableMultiDofg@ii)gQ?g{Gzg
ףp=
)cameraDistance	cameraYawcameraPitchcameraTargetPositioni'     )numSolverIterationsgO贁N[?)fixedTimeStepz%/data/motions/humanoid3d_backflip.txtzpath	=	rzlen motion=LoopFramesz
#frames = frameerpg?kpMotor
forceMotori    )JOINT_REVOLUTEJOINT_PRISMATICJOINT_SPHERICALJOINT_PLANARJOINT_FIXED)r   r      )r   r   r   )r   r   )r   r   )r   r      z	Stable PDr   zSpherical	DrivezExplicit PD	Kinematic   zStable PD (Py)r   zhumanoid/humanoid.urdfg      ?)globalScalinguseFixedBaseflags)r   r   r   r   )+r   gm?r   gs+?gvp?gG?FgMgND[G?gvďl?gOMYgaTrr   r   r   r   gQ&?gw4/?gBZ߁wgyP?gtxϿgN?gO"a?gK?gYGB?g*Y3?g
aG|gp:#޷?g&*gRAE?g/A?ggg*?gd}xg$;ٿgQXs?ggr?gw/=g'Nb?gpv?g8pJl?gŸ]5¿gtG??gN
g?)+g?g}uuggGPe?g\agQ?g溿r   g1FN?g<	N0?g;7?r   r   r   r   r   gm0?gN(?g0j&ܿr   g"gY?K+gէ9g`gHdWn̿r   g9*ǿg)\?gm?r   gީ?gb1g?g]?gm$?r   gŁqgyXk'g^xtgJ~r   g5
Ygtڪ?g"O(r   gu   zspherical position: zspherical velocity: )targetValuetargetVelocityz	revolute:zrevolute	velocity:)r&   positionGainvelocityGainforce)   r*   r*   )r   r   r   )r&   r)   r*   
   zjoint[z].type=z].name=)r   r   r   r   )	rgbaColor)r   r   r   r         	            )+r   r   r   r   r   r   r   r   r   r   r   d   r3   r3   r3     r4   r4   r4   r4     r5   r5   r5   r5   r5   r5   r5   ,  r4   r4   r4   r4   r4   r5   r5   r5   r5   r5   r5   r5   r5   r6   )+r   r   r   r   r   r   r   r3   r3   r3   r3   r+   r+   r+   r+   2   r7   r7   r7   r7   (   r8   r8   r8   r8   r8   r8   r8   r
   r7   r7   r7   r7   r7   r8   r8   r8   r8   r8   r8   r8   r8   r
   i@     )r   r           )gQ?r   r   )r   r   r:                                                          !   "   $   %   &   #   r8   )   *   '   +   i)r   r   r   r   r   r   r   r   r   r   r   r   )bodyUniqueIdjointIndicesdesiredPositionsdesiredVelocitieskpskds	maxForcestimeStepr3   )targetPositionspositionGainsvelocityGainsforcesg?)r)   )controlModer)   )targetPositionr'   r)   zjointStateMultiDof[z].pos=z].vel=z].jointForces=)pybulletpjsontimepybullet_datauseGUIconnectGUIDIRECTsetAdditionalSearchPathgetDataPathuseZUpuseYUpshowJointMotorTorquesconfigureDebugVisualizerCOV_ENABLE_Y_AXIS_UP&pybullet_examples.pdControllerExplicitr   $pybullet_examples.pdControllerStabler   
explicitPDstablePDresetDebugVisualizerCamera
setTimeOutuseMotionCaptureuseMotionCaptureResetuseExplicitPDsetPhysicsEngineParameterr^   pathprintopenfloadmotion_dictlen	numFramesaddUserDebugParameterframeIderpId	kpMotorIdforceMotorId
jointTypesstartLocationsaddUserDebugTextURDF_MAINTAIN_LINK_ORDERURDF_USE_SELF_COLLISIONr"   loadURDFhumanoid	humanoid2	humanoid3	humanoid4	humanoid5createConstraintr   humanoid_fixhumanoid2_fixhumanoid3_fixhumanoid4_fix	startPosestartVelresetBasePositionAndOrientationindex0rangegetNumJointsjgetJointInfojird   	jointTyper   	targetVelresetJointStateMultiDofr   r   setJointMotorControlMultiDofPOSITION_CONTROLsetJointMotorControl2VELOCITY_CONTROLjointIdsparamIdschangeVisualShapeinfo	jointNameappendchestneckrightHip	rightKnee
rightAnklerightShoulder
rightElbowleftHipleftKnee	leftAnkleleftShoulder	leftElbowkpOrgkdOrgoncegetCameraImageisConnectedreadUserDebugParameterr   r   maxForce	frameRealkpintr   	frameNextframeFraction	frameDataframeDataNextbasePos1StartbasePos1EndbasePos1baseOrn1StartbaseOrn1NextgetQuaternionSlerpbaseOrn1y2zPosgetQuaternionFromEulery2zOrnmultiplyTransformsbasePosbaseOrnchestRotStartchestRotEndchestRotneckRotStart
neckRotEndneckRotrightHipRotStartrightHipRotEndrightHipRotrightKneeRotStartrightKneeRotEndrightKneeRotrightAnkleRotStartrightAnkleRotEndrightAnkleRotrightShoulderRotStartrightShoulderRotEndrightShoulderRotrightElbowRotStartrightElbowRotEndrightElbowRotleftHipRotStartleftHipRotEnd
leftHipRotleftKneeRotStartleftKneeRotEndleftKneeRotleftAnkleRotStartleftAnkleRotEndleftAnkleRotleftShoulderRotStartleftShoulderRotEndleftShoulderRotleftElbowRotStartleftElbowRotEndleftElbowRot
setGravityjointDofCounts	totalDofsdofjointIndicesAllgetBasePositionAndOrientationpose	computePDtausindicesr_   r]   r[   r\   !setJointMotorControlMultiDofArraySTABLE_PD_CONTROLtaus3dofIndexindex
jointIndexTORQUE_CONTROLkinematicHumanoid4stepSimulationgetJointStateMultiDof
jointStatesleep     a/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/humanoidMotionCapture.py<module>r     s       
	))AEE))AHH   -335 6	 	Q33Q7 O K)!,
%a(   C'*),2FH
  U      -335 6    3    ( 3  "%LL
 i 	$_		!+  mS% & k& H%&	 lI 

!
!'1i!mQ
?	q!S1##Iq!R8	&&|QdC
 Y
J	J   ;"1%a(.*;A*>*BNSTDUVWDXY   $"1%a(.*;A*>*BNSTDUVWDXY   ="1%a(.*;A*>*BNSTDUVWDXY   ;"1%a(.*;A*>*BNSTDUVWDXY   #"1%a(.*;A*>*BNSTDUVWDXY   !:!::::.$Q'$(#(!	#
 JJ/%a(%)$)"	$	
 JJ/%a(%)$)"	$	
 JJ/%a(%)$)"	$	
 JJ/$Q'$(#(!	#	 !!(BByR["0"3\C""9b"b!--T]#1!#4lD""9b"b!--T]#1!#4lD""9b"b!--T]#1!#4lD""9b"b!--T]#1!#4lD	  ! !(N1,=| L  ! !)^A-> M  ! !)^A-> M  ! !)^A-> M  ! !)^A-> M	
	q~~h'	(A~~h""3.e)1$$$&1*y!4i
6KYW]`aWaMbN &1*%x
';Xfqj=QRI
aKF	
 .1	
 ),h~V_`iW`aiW`a1$$$	Q5E5E(E'(N&!"I
aKF	+~&	
	*h~V_`iW`aiW`a/ 
)2 
q~~h'	(A~~h""3.e)1$$$!N""8#$#$#5#5#12;0101)24 ""9#$#$#5#5#12;0101)24 ""9#$#$#5#5#12;0101)57 ""9#$#$#5#5#12;0101)24 1$$$	Q5E5E(EHa););AUVWIq!*<*<QVXYIq!*<*<QVXYIq!*<*<QVWX !Y
2a5 12!Y1&_ 
)b 	q~~h'	(Ah\:	
!	$$

QIQIQ&&&)q7G7G*Gooa 
)   Ha< 8		


		 	
	
    c }}

"
"5
)C&&y1G''5H((1I
CGHI"
i.%ai)9Ie#- (#E*)h'	2-
 Q<1y|<-q!=#3]15EF+A+a.=;K*KLLA+a.=;K*KLLA+a.=;K*KLL(
 Q<1y|Yq\J-"M!$4mA6FVWHXY,!!-}M(F%%l3F++FFHhOGW%%hAF%%l3F++FFHhOGW%%i'BQ<2	"y|L-q!=#4mB6GWXIYZ+!!-mL(B-2	"y}M,b!=#4mB6GWYIZ[*  z=I'mYr]IbM9R=Q!"%}R'8-:K][]M^_.$$%5~}U+ }o"2&'/]oa.@CTUVCW.WXX, ""y}imYr]S#B'r):M"<M}]_O`a&&'9;K][-$R=)B-2	RTVBr*M",=}R?P ))*?AT*79 ""#B'(m/?/BEWXYEZ/Z[[- r]IbM9R=)B-P/ $mB&7r9JMZ\L]^-##O]MR*m_!"%&.!!$}q8IL\]^L_8_'``a+ }imYr]IbMR"2&b(9=;Lm\^N_`/%%&7-X,#B-2	"yQS}U%b)=+<mB>OQ^_aQbc(()=?QS`a/ }o"2&'/]oa.@CTUVCW.WXX,  
$,,q!S"9NI3i  	tXy*mZQXZb<O 66x@GW


AJ
 '

,3AJ
8?

DKAJ
PWXYPZ

a[
"*1+
/7{
<CAJ
HOPQ

T[\]T^
 	

  N
 %0N
 5@N
 EPPQN
 	Q	
 'q)	
 ,9+;	
 >K1=M	
 P]]^O_	

 	

 .a0

 3C12E

 HXXYGZ
 	a
 %Q-
 *4A
 9C1
 HRRS}
 	A
 %Q
 *6a
 ;Gq/
 LXXY?
 	
 ,A.
 1@0B
 ETTUDV
 	Q
D" 9-<1534#	/$)$)+3*y*@)1  3D dHi-)Y(G  \#%5} +|%|5O Xh/(8H1MxX`aiNjltkuHX.(/KXJHX.
Xhx<XHX.
<I &)C%(C ''#*#$#6#63B1414*35   i.=26453?%*%*,4tO+<y+H*2 ! 4E Hs?+,"5)j		!	#	&&1%tHqL'941;MN		P
 	
&&A&hl(;U8a<=PQ		S 
	!	# 	
&&y'1343C3C.28n-=	? 	
&&y'1343C3C.3Ho->	@
 .''h7 -@   !&!"!3!308.0(0z3   !%!"!3!307.0(0z3   !)!"!3!30;.0(0z3   !*!"!3!30<.0(0z3   !+!"!3!30=.0(0z3   !.!"!3!30@.0(0z3   !+!"!3!30=.0(0z3   !(!"!3!30:.0(0z3   !)!"!3!30;.0(0z3   !*!"!3!30<.0(0z3   !-!"!3!30?.0(0z3   !*!"!3!30<.0(0z3 i9iw7i;?iLAi]Ci8HIi]Ci*=i;?iLAiGiLA1>>),-**9a8j!1h
1>!1h
1>!1&6
1F	 .
 **XW
 	}}K _s   AJJ
AJ 