
    ni`                         S SK r S SKrS SKrS rSrS r\(       a  S SKrS rSr	Sr
S/\
-  rS/\
-  rS/\
-  r/ SQr\r\r " S S\5      rg)	    N      i   )r   r   r   r   r   r   c                   &    \ rS rSrS rS rS rSrg)XArm6Sim   c           	         [         (       aO  [        R                  " 5       U l        U R                  R	                  5       U l        [        SU R
                  5        Xl        [        R                  " U5      U l
        S/[        -  U l        U R                  R                  n/ nU R                  R                  SSUS   -   US   US   // SQUS9  UR                  U R                  R                  S	[        R                  " / S
Q5      U R                  -   US95        UR                  U R                  R                  S	[        R                  " / SQ5      U R                  -   US95        UR                  U R                  R                  S	[        R                  " / SQ5      U R                  -   US95        U R                  R                  S[        R                  " / SQ5      U R                  -   US9nU R                  R                  S[        R                  " / SQ5      U R                  -   US9  U R                  R                  S[        R                  " / SQ5      U R                  -   US9  / SQnU R                  R                  S[        R                  " / SQ5      U R                  -   USUS9U l        Sn[#        U R                  R%                  U R                   5      5       H  nU R                  R'                  U R                   USSS9  U R                  R)                  U R                   U5      n	U	S   n
U	S   nXR                  R*                  :X  a3  U R                  R-                  U R                   U[.        U   5        US-   nXR                  R0                  :X  d  M  U R                  R-                  U R                   U[.        U   5        US-   nM     SU l        g )NznumJoints IKFast=r   ztray/traybox.urdf皙?r      )r   r   r   r   )flagszlego/lego.urdf)333333?皙?r   )r   gr   )      ?r   r   zsphere_small.urdf)r
   r   r   )r   r   r   )r   r   r   zxarm/xarm6_robot.urdf)r   r   r   T)useFixedBaser   )linearDampingangularDamping        )	useIKFastikfastpyPyKinematicsxarm_kingetDOFn_jointsprintbullet_clientnparrayoffsetxarmNumDofs
jointPoses"URDF_ENABLE_CACHED_GRAPHICS_SHAPESloadURDFappendxarmrangegetNumJointschangeDynamicsgetJointInfoJOINT_PRISMATICresetJointStatejointPositionsJOINT_REVOLUTEt)selfr   r   r   legossphereIdornindexjinfo	jointName	jointTypes               W/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_robots/xarm/xarm_sim.py__init__XArm6Sim.__init__   s3   y++-dmmm**,dm/&((6"DKc+oDOAAE
E 3c&)mVAYPVWXPY5Z\emrs	LL##,,-=bhh>WX\XcXc>ckp,qr	LL##,,-=bhhGW>XY]YdYd>dlq,rs	LL##,,-=bhh>WX\XcXc>ckp,qr!!**+>rxx?WX\XcXc?ckp*qH 3BHHm4LT[[4X`ef 3BHHm4LT[[4X`efC""++,CRXXgEVW[WbWbEbdgvz  CH+  IDIE4%%22499=>
''		1AVW'X,,TYY:dq'iq'i
))99
9**499a9NOAg
))88
8**499a9NOAg ? DF    c                     g )N )r.   s    r7   resetXArm6Sim.reset>   s    r:   c                 8   U R                   nU =R                   S-  sl         SU R                  S   -   S[        R                  " SU-  5      -  -   SU R                  S   -   S[        R                  " SU-  5      -  -   SU R                  S	   -   /n/ S
Qn[
        (       Gaj  / SQnU R                  R                  S5        U R                  R                  U5      nU R                  R                  5         [        R                  " U5      R                  SS5      nU R                  R                  S5        U R                  R                  UR                  S5      R                  5       5      nU R                  R                  5         [        [!        U5      U R"                  -  5      n[%        SU-  5        [        R                  " U5      R                  XpR"                  5      nU H  n[%        U5        M     S/U R"                  -  n	O[&        (       a  U R                  R)                  U R*                  [,        X#[.        [0        [2        [        R4                  " U R6                  5      R                  5       SSS9
n	SSU	S	   U	S   U	S   U	S   /U l        O*U R                  R)                  U R*                  [,        X#SS9n	[8        (       aW  [;        [<        5       HC  n
U R                  R?                  U R*                  U
S-   U R                  R@                  X   SS9  ME     OB[;        [<        5       H/  n
U R                  RC                  U R*                  U
S-   X   5        M1     U R                  RE                  U R*                  [,        SS9n[        R4                  " US   5      nUS   n[        R4                  " US   5      nUS   nU R                  RG                  U5      nU R                  RI                  X./ SQSS9  U[        R4                  " U5      -
  ng )Ng?g?5^I?r   g?g      ?r   r   gQ?r   )r   r   r   r   )g(\?r   r   r   r   r   zikfast.forwardDynamics      zikfast.inverseDynamicsz%d solutions found:gh㈵>2   )lowerLimitsupperLimitsjointRanges	restPosesresidualThresholdmaxNumIterations   )rI   g     @)forceT)computeForwardKinematics)r   r   r   r   d   )lifeTime)%r-   r   mathsincosr   r   submitProfileTimingr   forwardr   asarrayreshapeinversetolistintlenr   r   useNullSpacecalculateInverseKinematicsr$   xarmEndEffectorIndexlluljrr   r    useDynamicsr%   r   setJointMotorControl2POSITION_CONTROLr*   getLinkStategetMatrixFromQuaternionaddUserDebugLine)r.   r-   posr1   joint_anglesee_posejoint_configsn_solutionsjoint_configr    ils
linkComPos
linkComOrnlinkUrdfPoslinkUrdfOrnmatdiffs                     r7   stepXArm6Sim.stepA   sm   AFFfFQdhhsQw&7 77T[[^9KCRVRZRZ[^ab[bRcLc9ceijnjujuvwjxex
yC
Cy &l
,,-EF%%l3g
,,.

7#++Aa0g
,,-EFmm++GOOB,?,F,F,HIm
,,.M*4==89k!;/0jj/77MMRm',l (3t}}$j 
''BB499MacfxzR288DOO3L3S3S3Uhl  @B C C
Qz!}Z]:a=TUW''BB499Macf  AB  B
{[!!



2
2499ac4CUCUCfCfhrhu  }E
2  F " [!!**499ac:=I "				(	(4Hcg	(	hBxx1J!uJAK1K 


4
4[
AC'')S'Qrxx}$Dr:   )r   r    r   r   r-   r$   r   N)__name__
__module____qualname____firstlineno__r8   r=   rt   __static_attributes__r<   r:   r7   r   r      s    $J	7%r:   r   )timenumpyr   rO   rZ   r`   r   r   ikSolverr\   r   r]   r^   r_   r+   rpr    objectr   r<   r:   r7   <module>r      st      	 	U;T+T+
`%v `%r:   