
    niZ              	          S SK rS SKrSrS SKr\(       a  S SKJr  S SK	r	Sr
/ SQr/ SQrSr\R                  " \R                  5      r\R"                  " \5        \R$                  " \R&                  " 5       5        Sr\(       a  S S/r\R,                  " S\\SS	9rOS S
/r\R,                  " S\\SS	9r\R0                  " \SS S S9  \R0                  " \S S S S9  \R0                  " \S
S S S9  / SQr\R4                  " \\\R6                  SS/S9  SrSr\" \\-
  \-  5      rS /\-  r S/\-  S/\-  /r!S/\-  S/\-  /r"S/\-  S/\-  /r#\$" \5       GHc  r%\\%\-  -   \ \%'   SS\	RL                  -  -  \	RN                  " S\	RL                  -  \ \%   -  5      -  \ \%   -
  \!S    \%'   SS\	RL                  -  -  \	RP                  " S\	RL                  -  \ \%   -  5      S-
  -  \!S
   \%'   \	RP                  " S\	RL                  -  \ \%   -  5      S-
  \"S    \%'   \	RN                  " S\	RL                  -  \ \%   -  5      \"S
   \%'   S\	RL                  -  \	RN                  " S\	RL                  -  \ \%   -  5      -  \#S    \%'   S\	RL                  -  \	RP                  " S\	RL                  -  \ \%   -  5      -  \#S
   \%'   GMf     S/\-  S/\-  /r)S/\-  S/\-  /r*S/\-  S/\-  /r+\$" \," \ 5      5       GHe  r-\R\                  " \\5      r/\/S    S    \)S    \-'   \/S
   S    r0\
(       a  \1" S5        \1" \/S
   S    5        \0\)S
   \-'   \/S    S
   \*S    \-'   \/S
   S
   \*S
   \-'   \)S    \-   \)S
   \-   /r2\*S    \-   \*S
   \-   /r3\#S    \-   \#S
   \-   /r4\
(       aH  \1" S5        \1" S5        \1" \5        \1" S5        \1" \25        \1" S5        \1" \35        \1" S5        \1" \45        \Rj                  " \\2\3\45      r6\6S    \+S    \-'   \6S
   \+S
   \-'   \
(       a  \1" S5        \1" \65        \R4                  " \\\Rn                  \6S    \6S
   /S9  \Rp                  " 5         GMh     \(       GaP  \Rr                  " SS/S9r9\9Ru                  SS S!S"S#S$S%9  \9Rw                  S&5      r<\<R{                  S'5        \<R	                  \ \!S    S(S)S*S+9  \<R	                  \ \!S
   S,S)S-S+9  \<R	                  \ \)S    S.S
S/S+9  \<R	                  \ \)S
   S0S
S1S+9  \<R}                  SS5        \<R                  5         \9Rw                  S25      r@\@R{                  S35        \@R	                  \ \"S    S(S)S*S+9  \@R	                  \ \"S
   S,S)S-S+9  \@R	                  \ \*S    S.S
S/S+9  \@R	                  \ \*S
   S0S
S1S+9  \@R}                  SS5        \@R                  5         \9Rw                  S45      rA\AR{                  S55        \AR	                  \ \#S    S(S)S*S+9  \AR	                  \ \#S
   S,S)S-S+9  \AR}                  S6S75        \AR                  5         \9Rw                  S85      rB\BR{                  S95        \BR	                  \ \+S    S.S:S;S+9  \BR	                  \ \+S
   S0S:S<S+9  \BR}                  S=S>5        \BR                  5         \R                  " S?5         \Rp                  " 5         \R                  " S?5        M%  )@    NTF)        r   r   )r   r   r         ?g-C6?   z TwoJointRobot_w_fixedJoints.urdf)useFixedBase   z!TwoJointRobot_wo_fixedJoints.urdf)linearDampingangularDamping)JOINT_REVOLUTEJOINT_PRISMATICJOINT_SPHERICALJOINT_PLANARJOINT_FIXEDJOINT_POINT2POINT
JOINT_GEARr   )forcesr   g       @g      g       zjoint_states[1][0]calculateInverseDynamicsid_robotobj_posobj_velobj_accztorque=   g      @)figsizeg?g)\(?g
ףp=
?g?g?g?)leftbottomrighttopwspacehspace   zJoint Positionz--r   z
Desired q0)lwlabelz--bz
Desired q1z-rzMeasured q0z-bzMeasured q1   zJoint Velocity   zJoint Accelerationg      $g      $@   zExecuted Torque   z	Torque q0z	Torque q1g      4g      4@g{Gz?)Epybulletbulletpybullet_datapdplottimematplotlib.pyplotpyplotpltmathverbose
robot_baserobot_orientationdelta_tconnectGUIid_simulatorsetTimeStepsetAdditionalSearchPathgetDataPathwith_fixed_jointsid_revolute_jointsloadURDFr   changeDynamicsjointTypeNamessetJointMotorControlArrayVELOCITY_CONTROLstartendintstepstq_pos_desiredq_vel_desiredq_acc_desiredrangespisincosq_posq_velq_torlenigetJointStatesjoint_statesaprintr   r   r   r   torqueTORQUE_CONTROLstepSimulationfiguresubplots_adjustadd_subplotax_pos	set_titleset_ylimlegendax_velax_accax_torpausesleep     \/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/inverse_dynamics.py<module>rk      s~     ! 
 
$ 
 ~~fjj)   7    r~~/ 0  1v__?'.*.0(
 1v__@'.*.0(
   h!A F   h1 E   h1 E     !3!'!8!8),c
4 		S5[G#$C%Kte|,te|,te|,	uA	W	!A$b477l+dhhrDGG|ad7J.KKaPQdR-1rDGG|,dgg!9L0MPS0ST-1dgg!!45:-1dgg!!45-1dggdgg!1D(EE-1TWWtxxTWWqt0C'DD-1 
 te|$
te|$
te|$ 
s1vA &&x1CD,Q"%(1+1oa!	
	,q/!
%(1+Q"%(1+Q"%(1+ 1Xa[%(1+&'1Xa[%(1+&'1a -"21"56'	
$%	*	(O	)	'N	)	'N	)	'N**8WgwO&q	%(1+q	%(1+	)	&M 	""8#5#)#8#8+1!9fQi*@B 	[ 
` ::r3i(&d4tSY]^c"&#$++aq!5Ql+C++aq!5Ql+C++aq4A]+;++aq4A]+;//#r--/c"&#$++aq!5Ql+C++aq!5Ql+C++aq4A]+;++aq4A]+;//#r--/c"&'(++aq!5Ql+C++aq!5Ql+C//$--/c"&$%++aq4A[+9++aq4A[+9//$--/))D/**T 	ri   