niZSrSSKJr SSKJr SSKJr SSKrSSKrSrSS\RS\R\ S \ S \R\ S \R\ S \R\ 4 S jjr S\RS \ 4SjrS\RS \ 4SjrS\RS \ 4Sjrg)z The inverse kinematic utilities.)absolute_import)division)print_functionNrrrrobot link_positionlink_id joint_idsbase_translation base_rotationcZU(daUR5UR5pURRXxXV5upxURRXxU[5upOUn Sn URR UR X)U S9n UV s/sHoU PM nn U$s sn f)ayUses Inverse Kinematics to calculate joint angles. Args: robot: A robot instance. link_position: The (x, y, z) of the link in the body or the world frame, depending on whether the argument position_in_world_frame is true. link_id: The link id as returned from loadURDF. joint_ids: The positional index of the joints. This can be different from the joint unique ids. position_in_world_frame: Whether the input link_position is specified in the world frame or the robot's base frame. base_translation: Additional base translation. base_rotation: Additional base rotation. Returns: A list of joint angles. r)solver)GetBasePositionGetBaseOrientationpybullet_clientmultiplyTransforms_IDENTITY_ORIENTATIONcalculateInverseKinematics quadruped)rr r r position_in_world_framer r base_positionbase_orientationworld_link_pos_ ik_solverall_joint_anglesi joint_angless l/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/utilities/kinematics.pyjoint_angles_from_link_positionr! s2 !&+&;&;' !$&+&;&;&N&N)9'J#M--@@8MONA#N)**EE oowyFB099y!1%y,9 :sB(c|[R"URRURU5S5$)zComputes the link's position in the world frame. Args: robot: A robot instance. link_id: The link id to calculate its position. Returns: The position of the link in the world frame. r)nparrayr getLinkStater)rr s r link_position_in_world_framer&<s6  (('B1E GGc:UR5UR5p2URRX#5upEURR UR U5nUSnURR XEUS5up[R"U5$)zComputes the link's local position in the robot frame. Args: robot: A robot instance. link_id: The link to calculate its relative position. Returns: The relative position of the link. rr) rrrinvertTransformr%rrr#r$) rr rrinverse_translationinverse_rotation link_stater link_local_positionrs r link_position_in_base_framer.Ms%*$9$9% "*/*?*?*O*O+''$$11%//7K*Q-- 00CC]LJ % &&r'cURVs/sHo"SPM nnS/[U5-nURRURUSUXD5upV[ R "U5nURSS:XdeU$s snf)aComputes the Jacobian matrix for the given link. Args: robot: A robot instance. link_id: The link id as returned from loadURDF. Returns: The 3 x N transposed Jacobian matrix. where N is the total DoFs of the robot. For a quadruped, the first 6 columns of the matrix corresponds to the CoM translation and rotation. The columns corresponds to a leg can be extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3]. rrrr) joint_stateslenrcalculateJacobianrr#r$shape)rr staterzero_vecjvrjacobians r compute_jacobianr:gs"-2,>,>?,>5Ah,>?S3'( ((    1 1%//72;=M2: F%"XXb\(  a   /@sB)Fr0r)__doc__ __future__rrrnumpyr#typingrAnySequencefloatintr!r&r.r:r'r rDs&&% $"/8,8, ::,??5),,s# , ooe, ,??5),^G ::G G"' ::' '4 :: r'