nijSrSSKJrJrJrJrJrJrJr SSK r SSK r SSK r SSK Jr SSKJr SSKJr SSKJr SSKJr SS KJr SS KJr SS KJr S rS r\ R<"SS\R>55r g)z"The base class for all quadrupeds.)AnyCallableDictSequenceTupleTextUnionN) bullet_client)sensor)hybrid_motor_model) robot_base) robot_config)robot_urdf_loader) data_types)kinematics_utils)rrr)rr cd\rSrSrSr\R SSS4S\RS\ S\ 4S\ RS \ RS \S \S \\R"S \R&4SjjrSHS\\ S\\ S\\\\ 4\\ 44SjjrSrSrSr\R<SIS\\ S\\ S\\\\ 4\\ 4S\4Sjj5r Sr!SJSjr"SKS\\\ \\\ 444Sjjr#Sr$SLS\S\%4Sjjr&SKS\S\ 4S jjr'SKS!jr(S"r)S#r*S$r+S%r,S&r-SMS'jr.SMS(jr/S)\\0Rb4S*jr2S+\%S)\0Rb4S,jr3S+\%S-\0RbS)\\%\ 44S.jr4\5S/5r6\7S05r8\7S15r9\7S25r:\7S35r;\7S45r<\7S55r=\7S65r>\7S75r?\7S85r@\7S95rA\7S:5rB\7S;5rC\7S<5rD\7S)\ R4S=j5rE\7S>5rF\7S?5rG\7S@5rH\7SA5rI\7SB5rJ\7SC5rK\7SD5rL\7SE5rM\7SF5rNSGrOg)N QuadrupedBasez7The basic quadruped class for both sim and real robots.Npybullet_clientclock.motor_control_mode motor_limitsmotor_model_class action_filtersensors safety_configc  zXlX lX0lXPlX@lSUlSUlX`lXplXl SUl [R"S5Ul UR5Ul[R"S5Ul[R"S5UlUR#5 g)abInitializes the class. Args: pybullet_client: The PyBullet client. clock: The sim or real clock. The clock function is typically provided by the gym environment. motor_control_mode: Specifies in which mode the motor operates. motor_limits: The motor limits of the robot. Used by the motor_model_class and action space building. motor_model_class: The motor model to use. Not needed for real robots. action_filter: The filter to smooth and/or regulate the actions. sensors: All sensors mounted on the robot. safety_config: The safety setting for the robot. **kwargs: Additional args. N)_pybullet_client_clock_motor_control_mode_motor_model_class _motor_limits _action_space _action_names_action_filter_sensors_safety_config _urdf_loadernpzeros_last_base_velocity_last_observation_time_last_base_acceleration_world%_last_base_acceleration_accelerometerload) selfrrrrrrrrkwargss f/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/quadruped_base.py__init__QuadrupedBase.__init__s8,K1/%DD'M'D!xx{D"&++-D)+!D&13!D.IIK base_positionbase_orientation_quaternion joint_anglesc UR5 UR(d#[R"URS9UlURR URR 5uUlUlURRSURS9uo@l URR5Ul URR5HxnURRURR UURRSSS9 URR!URR USSS9 Mz URR#5UlUR$(d ['S5eUR$R5Ul[+UR$5UlUR/5 UR0(aSUR1UR,UR2UR4R6UR4R8S9UlURR=UR$R?55Ul [BRD"[GUR@R555Ul$URRKUR$R?55Ul&[BRD"[GURLR555Ul'URQ5 URRHnURUU5 M g ) aLoads the URDF with the configured pose. Args: base_position: The base position after URDF loading. Will use the configured pose in gin if None. base_orientation_quaternion: The base orientation after URDF loading. Will use the configured values in gin if not specified. joint_angles: The desired joint angles after loading. Will use the configured values if None. )rrrrposition orientationr) bodyIndex jointIndex controlModetargetVelocityforce) linearDampingangularDampingz-Motor id dict cannot be empty for quadrupeds.) num_motorsrtorque_lower_limitstorque_upper_limitsN)+ _pre_loadr,rRobotUrdfLoaderr"getBasePositionAndOrientationrobot_id_init_urdf_position_init_orientation_quatinvertTransform_init_orientation_inv_quatget_joint_id_dict_joint_id_dictvaluessetJointMotorControl2VELOCITY_CONTROLchangeDynamicsget_motor_id_dict_motor_id_dict ValueError _motor_idslen _num_motors_build_action_spacer%r$r&rJrK _motor_modelget_joint_direction_dictkeys_motor_direction_dictr-arraylist_motor_directionsget_joint_offset_dict_motor_offset_dict_motor_offsets_on_loadr* set_robot)r4r:r;r<unused_positionjoint_idr s r6r3QuadrupedBase.loadGs" NN   +;;//1d ;;    & & (:Dd9 --D,G,G . I5O4 ++==?D''..0 11%%..++<< 2 **    $ $  +1"++==?D    F GG))002DO4../D 11%%!55"00DD"00DD 2d"&!2!2!K!K   ""$DXXd4+E+E+L+L+N&OPD"//EE   "$D((4(?(?(F(F(H#IJD MMO-- t r9cdUR[RR:Xa[R R URRURRUR4[RS9Ul [SURR!555UlgUR[RR$:Xa[R R URR&URR(UR4[RS9Ul [SURR!555UlgUR[RR*:XGaURRURR,SSURR&/nURRURR.[R0[R0URR(/n[R2"UR[R44U5R75n[R2"UR[R44U5R75n[R R X4[RS9Ul [SURR!555Ulg[9S5e) z4QuadrupedBase._build_action_space..s#!O3M%-  u % %3M c3D# UHnSRU5v M g7f)z TORQUE_{}Nrvrxs r6r{r|#!M1K+  U # #1Kr}r)rqrrrtc3D# UHnSRU5v M g7f)z HYBRID_{}Nrvrxs r6r{r|rr}Not yet implemented!N)r$rMotorControlModePOSITIONgymspacesBoxr&angle_lower_limitsangle_upper_limitsr_r-float32r'tupler[rcr(TORQUErJrKHYBRIDvelocity_lower_limitsvelocity_upper_limitsinffullHYBRID_ACTION_DIMENSIONravelNotImplementedError)r4hybrid_action_limits_lowhybrid_action_limits_high space_low space_highs r6r`!QuadrupedBase._build_action_spaces <#@#@#I#II::>>  33!!44!!# *d !!O373F3F3K3K3M!OOd ! !\%B%B%I%I I::>>  44!!55!!# *d !!M151D1D1I1I1K!MMd ! !\%B%B%I%I I    / /    2 2    0 0 "    / /    2 2BFFBFF    0 0# ''   \AA B "$$)EG77   \AA B #%%*UW::>> *r?N)raresetr-r.r/r#r0r1r2r__motor_torquesreceive_observation_reset_base_pose_reset_joint_anglesr"rNr,rOrPrQrRrS_time_at_reset)r4r:r;r<rr5rms r6rQuadrupedBase.resets6  "xx{D"&++-D)+!D&13!D.((4#3#34D -E\*    = =(( *nURU5 URR 5 UR 5 M@ UR 5 g)aUSteps the simulation. This is maintained for backward compatibility with the old robot class. Args: action: The control command to be executed by the robot. num_sub_steps: Each action can be applied (possibly with interpolation) multiple timesteps, to simulate the elapsed time between two consecutive commands on real robots. N)pre_control_steprange apply_actionr"stepSimulationrpost_control_step)r4rr_s r6stepQuadrupedBase.stepAs] " "6 *F = !  **,  " r9control_timestepc^UR(aURRU5nU$)aProcesses the action and updates per control step quantities. Args: action: The input control command. control_timestep: The control time step in the environment. TODO(b/153835005), we can remove this once we pass env to the robot. Returns: The filtered action. )r)filter)r4rrs r6rQuadrupedBase.pre_control_stepWs( ""))&1f Mr9cR[R"U5nURRX5up4X@l[R "X@R 5nURRURRURURRUS9 g)N)rB jointIndicesrDforces) r-asarrayraget_motor_torquesrmultiplyrgr"setJointMotorControlArrayr,rOr]TORQUE_CONTROL)r4motor_commandsrunused_observed_torquesactual_torquesapplied_motor_torquess r6rQuadrupedBase.apply_actiongsZZ/N ++NO,(KK8N8NO33##,,__))88$ 4&r9cURRURR5up[R "URUR RUUR5$r)r"getBaseVelocityr,rOrrotate_to_base_frame urdf_loaderrS)r4rangular_velocitys r6_get_base_roll_pitch_yaw_rate+QuadrupedBase._get_base_roll_pitch_yaw_rate{s`//?? ""$A  0 0 t//88:J '' ))r9chURRURR5upU$r)r"rr,rO)r4 base_velocityrs r6_get_base_velocity QuadrupedBase._get_base_velocitys0,,<< ""$M r9c>URUR:Ga[R"UR5UR - URUR- - UlURR[R"S5[R"UR55upURR[R"S5UUR [-[5Sn[R"U5Ulgg)z5Update the base acceleration using finite difference.r!rN)r0 timestampr-re_base_velocityr/r1rrRr.r;multiplyTransforms_GRAVITY_ACCELERATION_OFFSET_UNIT_QUATERNIONr2)r4rinv_base_orientationbase_acceleration_accelerometers r6_update_base_acceleration'QuadrupedBase._update_base_accelerations ""T^^3 ((4&& '$*B*B Bnnt:::,) positionA orientationA positionB orientationBrrN)r"rNr,rO_base_positionrrS_base_orientation_quatrrgetEulerFromQuaternion_base_roll_pitch_yawr_base_roll_pitch_yaw_rategetJointStatesr] _joint_statesr-re _motor_anglesrjrg_motor_velocitiesraupdater#rr/rr0)r4base_orientation_quatr joint_states r6r!QuadrupedBase.receive_observations ;;    & & (/D.&*%:%:%M%M44* &N&,"A" 113D $ 5 5 L L ##!%D&*%G%G%ID"..== ""DOO5D+/+=+=>+=KQ+=>@D,,--.151G1GHD XX+/+=+=>+=KQ+=>@D!33d6L6LLD  t{{}d.@.@#557 ""$!xx(;(;L;; < 2::d.A.A#BB  L  ,,r9c /n[URR5R55nUH~nU(d;UR [ R "URURUS95 MEUR [ R"URURUS95 M [R"U5$)z;Returns the robot's foot positions in the base/world frame.)rrlink_id) rr,rrVappendrlink_position_in_base_framerrOlink_position_in_world_framer-re)r4rrrfoot_ids r6rQuadrupedBase.foot_positionssN$++DDFMMOPM  $  8 8 $ 4 4      9 9 $ 4 4   ! 88N ##r9returnc2[URR5R55n[ [ U55Vs/sHn[ R"S5PM nnURRURRS9nUHnUupgpn ppnnnnnX:XaMX;a[ R"U 5U-n[ R"U5U-[ R"U5U--nUU-n[R"URURRUUR5n[ R R#U5nUR%U 5nUS:aUU==U- ss'MMM U$s snf)aGets the contact forces on all feet. Reals robot may use a robot specific implementation. For example, the Laikago will measure each contact force in the corresponding foot's local frame, and this force will not be the total contact force due to the sensor limitation. For simulated robots, we wll always report the force in the base frame. Returns: A list of foot contact forces. r!)bodyAr)rr,rrVrr^r-r.r"getContactPointsrOrerrrrSlinalgnormindex)r4rrcontact_forces all_contactscontact unused_flag body_a_id body_b_id link_a_idunused_link_b_idunused_pos_on_aunused_pos_on_bcontact_normal_b_to_aunused_distance normal_force friction_1friction_direction_1 friction_2friction_direction_2friction_forcerF local_forcelocal_force_normtoe_link_orders r6feet_contact_forces!QuadrupedBase.feet_contact_forcess$++DDFMMOPM+0]1C+DE+Dabhhqk+DNE((99((:*L  '{y5E)>Z!5z    #xx 56E "67*Drxx H"$.H//~-&;;  ! !4#3#3#<#>+6&,,Y7 a   (K 7 (  / 0 ;Fs Fleg_idc URRn[URR5R 55n[ R "URURX1URVs/sHoDSPM snS9[R"[R"U5UR/5-$s snf)aCompute the Jacobian for a given leg. Args: leg_id: Index of the leg for which the jacobian is computed. 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]. Note that the jacobian is calculated for motors, which takes motor directions into consideration. r)rrr all_joint_positions)r,com_dofrrrVrcompute_jacobianrrOrr- concatenateonesrg)r4r.r1rstates r6compute_jacobian_for_one_leg*QuadrupedBase.compute_jacobian_for_one_leg's''G$++DDFMMOPM  , ,,, %"&"4"4 "4!H"4  ^^RWWW-t/E/EF G  HH s?C  contact_forcec^[URR5R55nUR U5n[ R "X$5n0nUR[U5-nURRn[X-US-U-5H n XXU -Xi'M U$)zMaps the foot contact force to the leg joint torques. Args: leg_id: Index of the leg for which the jacobian is computed. contact_force: Desired contact force experted by the leg. Returns: A dict containing the torques for each motor on the leg. r) rr,rrVr6r-matmulrIr^r1r) r4r.r8rjvall_motor_torques motor_torquesmotors_per_legr1rns r6"map_contact_force_to_joint_torques0QuadrupedBase.map_contact_force_to_joint_torques?s$++DDFMMOPM * *6 2B -4M__M(::N''G&1!A:79 1H2D Em9 r9c[S5e)Nr)r)clss r6 get_constantsQuadrupedBase.get_constantsVs 4 55r9c"UR5$r)r#rs r6rQuadrupedBase.timestampZs ;;=r9cUR$r)r'rs r6 action_spaceQuadrupedBase.action_space^   r9cUR$r)r(rs r6 action_namesQuadrupedBase.action_namesbrJr9cUR$)aGets the base orientation as a quaternion. The base orientation is always relative to the init_orientation, which can be updated by Reset function. This is necessary as many URDF can have an internal frame that is not z-up, so if we don't provide an init_orientation (through Reset), the loaded robot can have its belly facing the horizontal direction. Returns: The base orientation in quaternion. )rrs r6r;)QuadrupedBase.base_orientation_quaternionfs  & &&r9chURRURR5upU$)aHGets the base orientation in the robot's default frame. This is the base orientation in whatever frame the robot specifies. For simulated robot this is the URDF's internal frame. For real robot this can be based on the rpy reading determined by the IMU. Returns: The base orientation in quaternion in a robot default frame. )r"rNr,rO)r4rrs r6)base_orientation_quaternion_default_frame7QuadrupedBase.base_orientation_quaternion_default_frameus6 ;;    & & (A ! r9cUR$r)r*rs r6rQuadrupedBase.sensorss ==r9cUR$r)rrs r6base_roll_pitch_yaw!QuadrupedBase.base_roll_pitch_yaws  $ $$r9cUR$r)rrs r6base_roll_pitch_yaw_rate&QuadrupedBase.base_roll_pitch_yaw_rates  ) ))r9cUR$r)rrs r6r:QuadrupedBase.base_position   r9cUR$r)rrs r6rQuadrupedBase.base_velocityr]r9cg)NTrrs r6is_safeQuadrupedBase.is_safes r9cUR$r)r_rs r6rIQuadrupedBase.num_motorss   r9cUR$r)rars r6 motor_modelQuadrupedBase.motor_model   r9cUR$r)r&rs r6rQuadrupedBase.motor_limitsrJr9cUR$r)rrs r6 motor_anglesQuadrupedBase.motor_anglesrJr9cUR$r)rrs r6motor_velocitiesQuadrupedBase.motor_velocitiess  ! !!r9cUR$r)rrs r6r=QuadrupedBase.motor_torquesr]r9cUR$r)r"rs r6rQuadrupedBase.pybullet_clients   r9cUR$r)r,rs r6rQuadrupedBase.urdf_loaderrhr9c.URR$r)r,rOrs r6rOQuadrupedBase.robot_ids    % %%r9cUR$r)rSrs r6'initital_orientation_inverse_quaternion5QuadrupedBase.initital_orientation_inverse_quaternions  * **r9cB[R"UR5$)aNGet the base acceleration measured by an accelerometer. The acceleration is measured in the local frame of a free-falling robot, which is consistent with the robot's IMU measurements. Here the gravitational acceleration is first added to the acceleration in the world frame, which is then converted to the local frame of the robot. )r-rer2rs r6r-QuadrupedBase.base_acceleration_accelerometers 88D>> ??r9cB[R"UR5$)z-Get the base acceleration in the world frame.)r-rer1rs r6base_accelerationQuadrupedBase.base_accelerations 88D66 77r9)%r)r(r'rrrrrr#rSrQrPrUrr2r1r/r0rr$rdrgr[r]r&rar%rirjrrr_r"r+r*rr,)NNN)NNNF)NNr)r)F)P__name__ __module__ __qualname____firstlineno____doc__r HybridMotorModelr BulletClientrfloatrr MotorLimitsrr sensor_libSensorsafety_data_types SafetyConfigr7rr rrr3r`rLrkgin configurableboolrrrrrintrrrrrrrrrrr-ndarrayr,r6r? classmethodrCpropertyrrHrLr;rQrrVrYr:rrarIrfrrlror=rrrOrzrr__static_attributes__rr9r6rrsH? 2BB-/6:,$11,c5j !,'77 , !,, ,  ,, ))*,'33,`%)26=A U5\U$)<U$tU{+U5\9: Un+8Z  %)26=A" :(5\:($)<:($tU{+U5\9: :(  :(:(x/ B DH7(-eEl.24;.?/@)A7  S,SE &() +$&1P @E->$*+8BJJ#7+ZHHH0(* 7;CJ7G.66        '  '  !  !   % % * *           L44    " "   ! !   & & + +  @  @ 8 8r9r)!rtypingrrrrrrr rrnumpyr-pybullet_utilsr &pybullet_envs.minitaur.envs_v2.sensorsr rpybullet_envs.minitaur.robotsr r rr$pybullet_envs.minitaur.robots.safetyrr'pybullet_envs.minitaur.robots.utilitiesrrrr RobotBaserrr9r6rsh(DDD (G<46;PD)~ 8J((~ 8~ 8r9