
    ni                        S SK rS SKrS rS rSS jrS r\R                  " \R                  5      r	\	S :  a  \R                  " \R                  5        \R                  " \R                  " 5       5        SrSr\R                  " 5         \R                   " \5        \R"                  " SS\5        \R$                  " S	/ S
Q5        \R$                  " SSS9r\R(                  " \/ SQ/ SQ5        \R*                  " \5      r\S-
  r\" \S/\-  5        \R0                  " 5         \" \5      u  rrr\" \5      u  rrr\R>                  " \\SSS9r \ u  r!r"r#r$r%r&r'r(S/\)" \5      -  r*\RV                  " \\\#\\*\*5      u  r,r-\." S5        \." \'5        \." S5        \." \" \\,\5      5        \." S5        \." \(5        \." S5        \." \" \\-\5      5        g)    Nc                    [         R                  " U [        [         R                  " U 5      5      5      nU Vs/ s H  o"S   PM	     nnU Vs/ s H  o"S   PM	     nnU Vs/ s H  o"S   PM	     nnX4U4$ s  snf s  snf s  snf )Nr         )pgetJointStatesrangegetNumJoints)robotjoint_statesstatejoint_positionsjoint_velocitiesjoint_torquess         T/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_examples/jacobian.pyr   r      s{    !!%q~~e/D)EF,+78<%1X</8,89L5AhL9)568-6	M	99 996s   A5A:!A?c                    [         R                  " U [        [         R                  " U 5      5      5      n[        [         R                  " U 5      5       Vs/ s H  n[         R                  " X5      PM     nn[        X5       VVs/ s H  u  pBUS   S:  d  M  UPM     nnnU Vs/ s H  oUS   PM	     nnU Vs/ s H  oUS   PM	     nnU Vs/ s H  oUS   PM	     nnXgU4$ s  snf s  snnf s  snf s  snf s  snf )Nr   r   r   )r   r   r   r	   getJointInfozip)	r
   r   ijoint_infosjr   r   r   r   s	            r   getMotorJointStatesr      s    !!%q~~e/D)EF,389N3OP3Oa)3O+P #L >L >!A$)! >,L+78<%1X</8,89L5AhL9)568-6	M	99 QL896s$    C"C'C'&C-:C2C7c           
         [         R                  " U 5      nS/U-  n[        U5      U:X  a:  [         R                  " U [	        U5      [         R
                  UUU/U-  U/U-  S9  g [        SR                  U[        [        5      5      5        g )N        )targetPositionstargetVelocitiespositionGainsvelocityGainsz?Not setting torque. Expected torque vector of length {}, got {})	r   r	   lensetJointMotorControlArrayr   POSITION_CONTROLprintformattorque)r
   positionkpkv
num_jointszero_vecs         r   setJointPositionr*      s    ~~e$*UZ(]j  %j 1 ! 2 20819/1dZ.?/1dZ.?A 
 $fZV=?    c                     / SQnSn[        [        U5      5       HO  n[        R                  " X5      S   S:  d  M"  [        S5       H  nX6==   X   U   X%   -  -  ss'   M     US-  nMQ     U$ )N)r   r   r   r   r   r   r   )r   r   r   r   )r
   jacobianvectorresultr   crs          r   multiplyJacobianr2   )   sl    &!Va~~e"R'Qx!	X[^fi//	 1fa	 
 
-r+   gMbP?gQ#r   z
plane.urdf)r   r   g333333ӿz TwoJointRobot_w_fixedJoints.urdfT)useFixedBase)r   r   r   )r   r   r   r   r   g?)computeLinkVelocitycomputeForwardKinematicsz.Link linear velocity of CoM from getLinkState:z8Link linear velocity of CoM from linearJacobian * q_dot:z/Link angular velocity of CoM from getLinkState:z:Link angular velocity of CoM from angularJacobian * q_dot:)g      ?g333333?)/pybulletr   pybullet_datar   r   r*   r2   connectSHARED_MEMORYclidDIRECTsetAdditionalSearchPathgetDataPath	time_stepgravity_constantresetSimulationsetTimeStep
setGravityloadURDFkukaIdresetBasePositionAndOrientationr	   	numJointskukaEndEffectorIndexstepSimulationposveltorqmposmvelmtorqgetLinkStater/   link_trnlink_rotcom_trncom_rot	frame_pos	frame_rotlink_vtlink_vrr   r)   calculateJacobianjac_tjac_rr"    r+   r   <module>r\      s    ::?" 	yy!1H))AHH   -335 6	      i  S#' ( 

< &	
6T	J
  ! !&)\ BNN6"	 1}  #* +      'S$'/ dE	
,,-12
4 PV L(GWiGW
 53t9""6+?$PXZbcu 6 7 g @ A vuc* + 7 8 g B C vuc* +r+   