ni6LSrSSKrSSKJrJrJr SSKrSSKrSSK J r Sr S\\\ 4S\\S\\\ 44S jr S \RS \RS \R4S jrS\RS \RS \R4Sjr\R""SS\55rg)zBHelper class to load and manage the robot URDF file in simulation.N)DictTextTuple) bullet_client robot_basejoint_name_to_id joint_namesreturncX[R"5nUcU$UH nXX#'M U$N) collections OrderedDict)rr sub_dictnames i/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/robot_urdf_loader.py _sub_dictrs4  $ $ &( Od%+HN /robot_space_joint_angles joint_offsetsjoint_directionscX-U-$r )rrrs rconvert_to_urdf_joint_anglesrs " 4} DDrurdf_space_joint_anglescX- U-$r r)rrrs rconvert_to_robot_joint_anglesr#s " 15E EErc \rSrSrSrS-S\R S\S\S\S\ \ S \ \ S \ \ S \ \S \ \\ 4S \ \\ 4S\ \\ 4S\ \S\ \S\ \\ \44Sjjr S.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44SjjrS.S\ \S\ \\ 44Sjjr\S5r\S5r\S5r\S5r\S 5rS!rS"rS#rS/S\S\ \ S \ \ S \ \\ 44S$jjrS\S\ \ S \ \ S\ 4S%jrS.S&\ \\ 44S'jjr S0S(\ \ S)\ \ 4S*jjr!\S+5r"S,r#g)1RobotUrdfLoader+z1A abstract class to manage the robot urdf in sim.Npybullet_client urdf_pathconstrained_baseenable_self_collisioninit_base_position init_base_orientation_quaternioninit_base_orientation_rpy base_namesinit_joint_anglesrr motor_namesend_effector_names user_groupc(XlX lXPlUbX`lO.Uc [ S5eURR U5UlX0lX@lXlXl Xl Xl Xl Xl XlURUUUU S9 g)aInitialize the class. Args: pybullet_client: A pybullet client. urdf_path: The path to the URDF to load. constrained_base: Whether to create a FIXED constraint to the base of the URDF. Needs to be True for kinematic robots. This allows us to "hang" the simulated robot in air, and the hanging point can follow arbitrary provided paths. enable_self_collision: Determines if the robot can collide with itself. init_base_position: The base x, y, z after loading. init_base_orientation_quaternion: The base rotation after loading. init_base_orientation_rpy: The base rotation after loading, presented in roll, pitch, yaw. base_names: The base joint names. Used to find additional links that belong to the base. Optional because the base might only contain a single mesh/block, which always has the id of "-1". init_joint_angles: Maps joint name to the desired joint pose after loading URDF. If not provided, will use the URDF default. This can be a subset of all joints in the URDF. This should be in the robot joint convention, which can be different from the URDF convention. joint_offsets: The "zero" position of joint angles in the URDF space. joint_directions: To convert between robot sdk/control and urdf joint convention. motor_names: The motor joint names in the URDF. Typically a subset of all movable joints/DoFs. end_effector_names: A subset of joints specifying the end-effector joint(s). For example for legged robots the end effectors are the toe joints (if provided). For arms this group includes left and right grippers. user_group: User defined joint groups. For example for quadrupeds, we may want to organize all joints according to which leg they belong to. NzOEither init_base_orientation_quaterion or init_base_orientation_rpy is required)r#r$r%r()_pybullet_client _urdf_path_init_base_position!_init_base_orientation_quaternion ValueErrorgetQuaternionFromEuler_constrained_base_enable_self_collision _base_names_init_joint_angles_joint_offsets_joint_directions _motor_names_end_effector_names _user_groupload)selfr r!r"r#r$r%r&r'r(rrr)r*r+s r__init__RobotUrdfLoader.__init__/sd,O1'3/O, " *DE E    6 6' ) ,."7!/'-#1!II3-)I+ -rrr cLUc UR$[URU5$r ) _base_dictrr=rs rget_base_id_dict RobotUrdfLoader.get_base_id_dict|s" | __ T__d ++rcLUc UR$[URU5$r )_joint_name_to_idrrBs rget_joint_id_dict!RobotUrdfLoader.get_joint_id_dicts& |  # ## T++T 22rcLUc UR$[URU5$r )_link_name_to_idrrBs rget_link_id_dict RobotUrdfLoader.get_link_id_dicts& |  " "" T**D 11rcLUc UR$[URU5$r ) _motor_dictrrBs rget_motor_id_dict!RobotUrdfLoader.get_motor_id_dicts& |    T%%t ,,rcLUc UR$[URU5$r )r8rrBs rget_joint_direction_dict(RobotUrdfLoader.get_joint_direction_dicts& |  # ## T++T 22rcLUc UR$[URU5$r )r7rrBs rget_joint_offset_dict%RobotUrdfLoader.get_joint_offset_dicts& |   T(($ //rcLUc UR$[URU5$r )_end_effector_dictrrBs rget_end_effector_id_dict(RobotUrdfLoader.get_end_effector_id_dicts& |  $ $$ T,,d 33rcUR$)zReturns the unique object instance id of this loaded URDF in pybullet. Note this is different from all other get_{}_id APIs, which returns the joint/link id within this robot instance. Returns: The object id as returned by loadURDF. ) _robot_idr=s rrobot_idRobotUrdfLoader.robot_ids >>rcUR$r )_all_joint_infor]s rall_joint_infoRobotUrdfLoader.all_joint_infos   rcUR$r ) _user_dictr]s r user_dictRobotUrdfLoader.user_dicts ??rcUR$r )r9r]s rr)RobotUrdfLoader.motor_namess   rcUR$r r3r]s rr" RobotUrdfLoader.constrained_bases  ! !!rc[R"5nURc SU['U$UR [ UR UR55 U$)aABuilds the base joints dictionary. In pybullet, a link's id within the robot always equal to its parent joint id. So this base joint dict functionaly is equivalent to the base link dict. The dictionary may only contain {ROBOT_BASE: -1} if self._base_names is empty. Returns: The base link (joint) ordered dictionary. )r rr5 ROBOT_BASEupdaterrF)r= base_dicts r_build_base_dict RobotUrdfLoader._build_base_dictsY'')I  i  y!7!79I9IJK rc[R"5nURcU$URR5HCup#[R"5X'XR [ UR U55 ME U$)z4Builds a dictionary using user defined joint groups.)r rr;itemsrprrF)r=rf group_namegroup_joint_namess r_build_user_dict RobotUrdfLoader._build_user_dicts{'')I  )-)9)9)?)?)A% )557i"" D**,= >@*B rc URRUR5n[U5Vs/sH)nURR URU5PM+ nnUH$nUSnURR USSSS9 M& [ R"5n[ R"[S4/5nUH6nUSRS5nUSnXVU'XWUSRS5'M8 XXg4$s snf)aExtracts all joints from the URDF. Finds all joints (fixed or movable) in the URDF and extracts the info. This includes actuated joints (i.e. motors), and non-actuated joints, e.g. the passive joints in Minitaur's four bar mechanism, and fixed joints connecting the toe and the lower legs, etc. Returns: number of joints, all joint information as returned by pybullet, and the joint_name_to_id dictionary. rrn) linearDampingangularDampingzUTF-8 ) r- getNumJointsr\range getJointInfochangeDynamicsr rrodecode) r= num_jointsirb joint_infojoint_idrlink_name_to_id joint_names r_build_all_joint_dict%RobotUrdfLoader._build_all_joint_dicts&&33DNNCJz""A **4>>1=" % Ah ** Ba+;% #..0!-- B/?.@AO$ a=''0jAh%-z"9Ajn++G45 % '7 HH+s0Dc Uc URnUc URnUc URnURXU5UlUR 5uUlUlUlUl UR5Ul [URUR5Ul[URUR5UlUR#5UlUR'X#5 UR)U5 g)z/Reloads the URDF and rebuilds the dictionaries.N)r4r/r0 _load_urdfr\r _num_jointsrarFrJrrrArr9rNr:rXrxrereset_base_posereset_joint_angles)r=r#r$r%r(s rr<RobotUrdfLoader.loads$"99!33'/)-)O)O&__%:%EGDN#88:Tt+T-C ++-DO !7!79J9JKD'(>(>(,(@(@BD++-DO+N-.rcU(aEURRURUUURURRS9$URRURUUURS9$! [ S5 [ S5 [ UR5 SSKnURS5 g=f)z+Loads the URDF and returns the pybullet id.) useFixedBaseflags)rz!!!!!!!!!!!!!!!!zError: cannot find file:rN)r-loadURDFr.r3URDF_USE_SELF_COLLISIONprintsysexit)r=r#r$r%rs rrRobotUrdfLoader._load_urdfs $$-- OO  ,//''?? .A A$$-- OO  ,// .    &' DOO hhqksA A>/A>>AC joint_anglescbURcgUc URnURb URc [S5eUR 5HYup#X0RU-URU-nUR R URURUUSS9 M[ g)a Resets the joint angles. Resets the joint poses. This is instanteneously and will ignore the physics (e.g. collisions, inertias, etc). Should only be used during the episode reset time. Does not work for real robots (other than changing the visualization). This API has no effect if both the input joint_angles and the self._init_joint_angles are None. Args: joint_angles: The joint angles in the robot's joint space. Raises: AttributeError if the joint directions and joint offsets are not provided during init. Nz;joint directions and joint offsets not provided in __init__r)targetVelocity) r6r8r7AttributeErrorrur-resetJointStater\rF)r=rrangleurdf_joint_angles rr"RobotUrdfLoader.reset_joint_angles7s  & ,,l %)<)<)D  G II*//1 !7!7 "++J78 ++ ..   ,  ,2r base_positionbase_orientation_quaternioncUc URnUc URnURRURX5 URR URSS5 g)zResets the base position and orientation. Instanteneously re-position the base pose of the robot. Does not work for real robots except for the visualization. Args: base_position: Base x, y, z position. base_orientation_quaternion: Base rotation. Nrrr)r/r0r-resetBasePositionAndOrientationr\resetBaseVelocity)r=rrs rrRobotUrdfLoader.reset_base_poseZsd..m"*$($J$J!99  D++DNNI,57rc,UR(aS$S$)Nrrkr]s rcom_dofRobotUrdfLoader.com_dofqs&&1-A-r)rarAr5r3r4rXr:r0r/r6r8rFr7rJrNr9rr-r\r.rer;) FTrNNNNNNNNNr )NNNN)NN)$__name__ __module__ __qualname____firstlineno____doc__r BulletClientrboolrfloatrintr>rCrGrKrOrRrUrYpropertyr^rbrfr)r"rrrxrr<rrrr__static_attributes__rrrrr+s;9 %$()27;04 $-1)-*.!%(,,0K-$11K-K- K- " K-  , K-).e K-"'uK-+K-dEk*K-$+&K-T3YK-;K- +K-tU4[()K-Z,5;,$tSy/, 3E$K34c ?3 25;2$tSy/2 -E$K-4c ?- 483%*4[3<@sO3150"'+09=dEk9J0484%*4[4<@sO4              " "$ #IN%))-7;-1 /!/ ,/).e / dEk* /:d%*5\38<DG4!T$+->!J%)2675\7$)<7. . .rr)rr typingrrrginnumpynppybullet_utilsrrorrndarrayrr configurableobjectrrrrrsH$$ ( T3Y ;+/c ?E jjE::EjjEFZZF::FjjFG.fG.G.r