
    ni                        S SK rS SKrS SKrS SKJr  S SKr\R                  " \R                  5      r\S :  a  \R                  " \R                  5        \R                  " \R                  " 5       5        \R                  " S/ SQ5        \R                  " S/ SQ5      r\R                  " \/ SQ/ SQ5        Sr\R                   " \5      r\S	:w  a  \" 5         / S
Qr/ SQr/ SQrS S S S\R,                  -  S \R,                  * S-  S-  S /r/ SQr\" \5       H  r\R6                  " \\\\   5        M     \R8                  " S S S 5        Sr/ SQr/ SQrS r Sr!Sr"Sr#S r$S r%\RL                  " \$5        Sr'S r \S-  r\$(       a1  \RP                  " 5       r)\)RT                  S-  S-  \R,                  -  rO\S-   r\#(       a  \$S :X  a  \RV                  " 5         \" S5       GH"  rSS\RX                  " \5      -  SS\RZ                  " \5      -  -   /r.\R^                  " S \R,                  * S /5      r0\!S:X  a7  \"S:X  a  \Rb                  " \\\.\0\\\\5      r2OH\Rb                  " \\\.\\\\S9r2O1\"S:X  a  \Rb                  " \\\.\0\\%SSS9r2O\Rb                  " \\\.\%S9r2\#(       a5  \" \5       H'  r\Rf                  " \\\Rh                  \2\   S SSSS9  M)     M  \" \5       H  r\R6                  " \\\2\   5        M     GM%     \Rj                  " \\5      r6\ (       a3  \Rn                  " \\./ SQS\'5        \Rn                  " \\6S    / S!QS\'5        \.r\6S    rSr GM  )"    N)datetimez
plane.urdf)r   r   g333333ӿzkuka_iiwa/model.urdf)r   r   r   )r   r   r            )glGzgRQ?r   gQ gffffff)gl?   Gz@gRQ@r
   gQ @gffffff@)333333@   r   r   r   r   r   g      ?gQ?)皙?r   r   r   r   r   r   g        r      g      N@g       @g{Gz?gٿg?)lowerLimitsupperLimitsjointRanges	restPosesd   )jointDampingsolvermaxNumIterationsresidualThreshold)r   i  gQ?)	bodyIndex
jointIndexcontrolModetargetPositiontargetVelocityforcepositionGainvelocityGain)r   r   g333333?r   )r   r   r   )9pybulletptimemathr   pybullet_dataconnectSHARED_MEMORYclidGUIsetAdditionalSearchPathgetDataPathloadURDFkukaIdresetBasePositionAndOrientationkukaEndEffectorIndexgetNumJoints	numJointsexitlluljrpirpjdrangeiresetJointState
setGravitytprevPose	prevPose1hasPrevPoseuseNullSpaceuseOrientationuseSimulationuseRealTimeSimulationikSolversetRealTimeSimulationtrailDurationnowdtsecondstepSimulationcossinposgetQuaternionFromEulerorncalculateInverseKinematics
jointPosessetJointMotorControl2POSITION_CONTROLgetLinkStatelsaddUserDebugLine
disconnect     ^/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/inverse_kinematics.py<module>r[      s       yy!1H))AEE   -335 6 

< &	
*I	6  ! !&)\ B NN6"	N& 3, AsTWW}a$''C$!6:(	y	AFAr!u% 
 Q1 	     - . Q$!
 	B	SB(A	DA-28atxx{"Btxx{):$:
;C

"
"Ax#3
4C
A
11&:NPSUXZ\^`24b:
 11&2F25>@>@>@<>@
 A
11&2F2525?A9ACFDGI
 11&2F259AC

 	Y!	&+,,-,>,>/9!}/0&)-1-.	0   Y!	&!Z]3  [ ` 	~~f23"xk1mDy"Q%A}E(e)+K rY   