
    ni                        S SK rS SKrS SKrS SKJr  S SKJr  S SKr\R                  " \R                  5      r\S :  a  \R                  " \R                  5        \R                  " S S9  \R                  " \R                  " 5       5        \R                  " S/ SQ5        \R                  " S/ SQ/ SQ5      r\" \R                   " \5      5       H  r\" \R&                  " \\5      5        M     \R                  " S	S
SSSSSS5      r\r/ SQr\" \R                   " \5      5       H  r\R0                  " \\\\   5        M     \R2                  " \S\S\R4                  / SQ/ SQ/ SQ/ SQ5	      r\R8                  " / SQ5      r/ SQrSr\R                   " \5      r\S:w  a  \ " 5         / SQr!/ SQr"/ SQr#S S S S\RH                  -  S \RH                  * S-  S-  S /r%/ SQr&\" \5       H  r\R0                  " \\\%\   5        M     \RN                  " S S S5        S r(/ SQr)/ SQr*S r+S r,S r-S!r.S!r/\R`                  " \/5        S"r1/ SQr2S r3S r3S# r4/ S$Qr5/ S%Qr6/ S&Qr7/ S'Qr8 \Rr                  " 5       r:S(r;/ S%Qr6S)r<\: GH;  r=\>" S*5      \:;   a  \R~                  " S+5        \>" S,5      \:;   a  \2S    \2S!   \;-
  \2S-   /=r2r2\>" S.5      \:;   a  \2S    \2S!   \;-   \2S-   /=r2r2\R                  \:;   a&  \" \A" \55      5       H  r\6\   \<\7\   -  -
  \6\'   M     \R                  \:;   a&  \" \A" \55      5       H  r\6\   \<\7\   -  -   \6\'   M     \R                  \:;   a&  \" \A" \55      5       H  r\6\   \<\8\   -  -   \6\'   M     \R                  \:;   d  GM  \" \A" \55      5       H  r\6\   \<\8\   -  -
  \6\'   M     GM>     \R8                  " S S \3/5      r\" \A" \55      5       H'  r\R                  " \\5\   \R                  \6\   S/S09  M)     \/(       a  \R                  " 5       r(O\(S1-   r(\.(       a  \/S :X  a  \R                  " 5         \" S!5       GH  rS2\R                  " \(5      -  S S S2\R                  " \(5      -  -   S3-   /rJ\R8                  " S \RH                  * S /5      rK\,S!:X  a7  \-S!:X  a  \R                  " \\\J\K\!\"\#\%5      rMOB\R                  " \\\J\!\"\#\%S49rMO+\-S!:X  a  \R                  " \\\J\K\&S59rMOS1rNS6rO\4" \\\J\N\O5      rM\.(       a5  \" \5       H'  r\R                  " \\\R                  \M\   S S7S!S8S99  M)     M  \" \5       H  r\R0                  " \\\M\   5        M     GM"     \R                  " \\5      rR\+(       a3  \R                  " \)\J/ S:QS!\15        \R                  " \*\RS;   / S<QS!\15        \Jr)\RS;   r*S!r+GMj  )=    N)datetime)enableConeFrictionz
plane.urdf)r   r   g333333ӿzhusky/husky.urdf)g~Ɍ?g8G?gσvӿ)gͮ{+c?gFn1PJDA?j?zkuka_iiwa/model_free_base.urdfgrPi?gH2w?gž?gJc?g3/Pr   r   )g`Hz@g=NP?g}?gL:?g uɳ?g0gY+x?)r   r   r   )        r   g      )r   r   r      )go!	@r   333333?      )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   ir   r	      c                    SnSnSnU(       d  Xd:  a  [         R                  " U [        U5      n[        [        5       H  n	[         R
                  " X	X   5        M     [         R                  " U [        5      n
U
S   nUS   US   -
  US   US   -
  US   US   -
  /nUS   US   -  US   US   -  -   US   US   -  -   nXs:  nUS-   nU(       d  Xd:  a  M  W$ )NFr   gꌠ9Y>)Fr   r	   r   )pcalculateInverseKinematicskukaEndEffectorIndexrange	numJointsresetJointStategetLinkState)kukaIdendEffectorId	targetPos	thresholdmaxItercloseEnoughiterdist2
jointPosesilsnewPosdiffs                i/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/inverse_kinematics_husky_kuka.py"accurateCalculateInverseKinematicsr+   Q   s    +	
$
%T^--f6JIVJ9:=1 	
 4	5BUFaL6!9$ilVAY&>	!vVWy@XYD!WtAwa47!22T!WtAw5FFE$K!8D T^ 
    )r      r      )r   r   r   r   )r	   r   r	   r   )r	   r	   r	   r	   g{Gz?g      ?szstate.pyar   di  )targetVelocityforcegMbP?g?gffffff?)lowerLimitsupperLimitsjointRanges	restPoses)jointDampingd   i  r   )	bodyIndex
jointIndexcontrolModetargetPositionr2   r3   positionGainvelocityGain)r   r   r
   r   )r	   r   r   )Tpybulletr   timemathr   pybullet_dataconnectSHARED_MEMORYclidGUIsetPhysicsEngineParametersetAdditionalSearchPathgetDataPathloadURDFhuskyr   getNumJointsr&   printgetJointInfor   objointPositionsr;   r   createConstraintJOINT_FIXEDcidgetQuaternionFromEulerbaseornr   r   exitlluljrpirpjd
setGravitytprevPose	prevPose1hasPrevPoseuseNullSpaceuseOrientationuseSimulationuseRealTimeSimulationsetRealTimeSimulationtrailDurationbaseposangr+   wheelswheelVelocitieswheelDeltasTurnwheelDeltasFwdgetKeyboardEventskeysshiftspeedkord	saveWorldB3G_LEFT_ARROWlenB3G_RIGHT_ARROWB3G_UP_ARROWB3G_DOWN_ARROWsetJointMotorControl2VELOCITY_CONTROLstepSimulationcossinposornr   r%   r    r!   POSITION_CONTROLr   r'   addUserDebugLine r,   r*   <module>r      sL        yy! 	1H))AEE   q 1   -335 6 

< &	

%'F<	>	q~~e$	%Aua ! 
&	
4h(T\x
3Xr*+JB
N:$>? ,
 FBy)Ub%' 
"
"#3
4
  NN6"	N& 3, AsTWW}a$''C$!6:(	y	AFAr!u% 
 Q3 	     - . 
$ 

  	
			$
% /
%a
3x4kk*
3x4"1:wqzE'971:FFg
3x4"1:wqzE'971:FFg4S[!!,Q/%/!:L2LL "D S[!!,Q/%/!:L2LL "~~S[!!,Q/%.:K2KK "4S[!!,Q/%.:K2KK "% * $$aC[1'VaE"1I..+:1+="&	(  		A 	
E	A-28a!acDHHQK&7!7#!=
>C

"
"Ax#3
4C
A
11&:NPSUXZ\^`24b:
 11&2F25>@>@>@<>@
 A
11&2F2525?A	C
 	7@TVY8A7L
 	Y!	&+,,-,>,>/9!}/0&)-.-0	2   Y!	&!Z]3  W \ 	~~f23"xk1mDy"Q%A}E(e)+ r,   