nidSrSSKJr SSKJr SSKJr SSKrSSKrSSKrSSKrSSK r SSK r SSK r SSK Jr SSK Jr SSK Jr SS KJr SS KJr SS KJr SS KJr /S Qr/SQr/SQr/SQr/SQrSrSr/SQr/SQr\ R@"S5r!\ R@"S5r"\ R@"S5r#\ R@"S5r$\ R@"S5r%\ R@"S5r&\ R@"S5r'Sr(Sr)Sr*S r+S!\RX-r-S!r.S"r/S#r0\ Rb"S$S%\255r3g)&zFThis file implements the functionalities of a minitaur using pybullet.)absolute_import)division)print_functionNminitaur_constants)minitaur_motor) robot_config)safety_checker) safety_error) action_filter) kinematics)rr皙?)rr)rrrr)rg{Gzt?r)r{Gz?rg@?) front_left back_left front_right back_right)motor_front_leftL_jointmotor_front_leftR_jointmotor_back_leftL_jointmotor_back_leftR_jointmotor_front_rightL_jointmotor_front_rightR_jointmotor_back_rightL_jointmotor_back_rightR_jointzchassis\D*centerz motor\D*jointzknee\D*zmotor\D*_bracket_jointz hip\D*jointz hip\D*linkz motor\D*link)rrrr)rrrrrrr)rrrrrrrrz:robotics/reinforcement_learning/minitaur/robots/data/urdf/cH[R"U5n[[U55Hsn[R "X[ 5X'X[R:aX==[ -ss'MJX[R*:dMcX==[ - ss'Mu U$)zMaps a list of angles to [-pi, pi]. Args: angles: A list of angles in rad. Returns: A list of angle mapped to [-pi, pi]. )copydeepcopyrangelenmathfmodTWO_PIpi)angles mapped_anglesis `/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/minitaur.pyMapToMinusPiToPir/4s~--'- V ayyF3M477"&   TWWH $&   c\rSrSrSr\\\SSS\RR\ RSSSS S \ S\\SSSSSS4S jrS rSmS jrS rSrSrSrSrSrSrSrSrSrSrSrSnSjrSr Sr!Sr"Sr#Sr$Sr%S r&S!r'S"r(S#r)S$r*S%r+S&r,S'r-S(r.S)r/S*r0S+r1S,r2S-r3S.r4S/r5S0r6S1r7S2r8S3r9S4r:S5r;S6rS9r?S:r@S;rAS<rBSmS=jrCS>rDS?rES@rFSArGSBrHSCrISDrJSErKSFrLSGrMSHrNSIrOSJrPSKrQSLrRSMrSSNrTSOrUSPrVSQrWSRrXSSrYSTrZSUr[SVr\SWr]SXr^SYr_SZr`S[raS\rbS]rcS^rd\eS_5rfS`rgSarhSbri\eSc5rj\eSd5rkSerlSfrmSgrnShro\eSi5rp\eSj5rq\rSk5rsSlrtg)oMinitaurGzHThe minitaur class that simulates a quadruped robot from Ghost Robotics.rrFrg{Gz?NrcX lURU-UlXlX`lX@lXplUUlUUl[R"UR5Ul [R"UR5Ul SUl Xl XlXl[ R""SS9Ul/UlS/Ul/Ul/Ul/UlUUlUUlUUlUR7UbUO [955 UUlSUlSUlUUl UUl!SUl"U (d [GS5eUR2(aUR4(a [GS5e[IU [ RJ[RL45(a[RN"U 5Ul(O[RR"X*5Ul([IU [ RJ[RL45(a[RN"U 5Ul*O[RR"X+5Ul*[IU [ RJ[RL45(a[RN"U 5Ul+OU cSUl+OXl+Xl,U "U U URVUS 9Ul-XPl.S Ul/S Ul0URRc/S QURe5S 9unUl3URB(aURi5Ul5URmS S9 URo5 g)a Constructs a minitaur and reset it to the initial states. Args: pybullet_client: The instance of BulletClient to manage different simulations. num_motors: The number of the motors on the robot. dofs_per_leg: The number of degrees of freedom for each leg. urdf_root: The path to the urdf folder. time_step: The time step of the simulation. action_repeat: The number of ApplyAction() for each control step. self_collision_enabled: Whether to enable self collision. motor_control_mode: Enum. Can either be POSITION, TORQUE, or HYBRID. motor_model_class: We can choose from simple pd model to more accureate DC motor models. motor_kp: proportional gain for the motors. motor_kd: derivative gain for the motors. motor_torque_limits: Torque limits for the motors. Can be a single float or a list of floats specifying different limits for different robots. If not provided, the default limit of the robot is used. pd_latency: The latency of the observations (in seconds) used to calculate PD control. On the real hardware, it is the latency between the microcontroller and the motor controller. control_latency: The latency of the observations (in second) used to calculate action. On the real hardware, it is the latency from the motor controller, the microcontroller to the host (Nvidia TX2). observation_noise_stdev: The standard deviation of a Gaussian noise model for the sensor. It should be an array for separate sensors in the following order [motor_angle, motor_velocity, motor_torque, base_roll_pitch_yaw, base_angular_velocity] motor_overheat_protection: Whether to shutdown the motor that has exerted large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more details. motor_direction: A list of direction values, either 1 or -1, to compensate the axis difference of motors between the simulation and the real robot. motor_offset: A list of offset value for the motor angles. This is used to compensate the angle difference between the simulation and the real robot. on_rack: Whether to place the minitaur on rack. This is only used to debug the walking gait. In this mode, the minitaur's base is hanged midair so that its walking gait is clearer to visualize. reset_at_current_position: Whether to reset the minitaur at the current position and orientation. This is for simulating the reset behavior in the real world. sensors: a list of sensors that are attached to the robot. safety_config: A SafetyConfig class to configure the safety layer. If None, the safety layer will be disabled. enable_action_interpolation: Whether to interpolate the current action with the previous action in order to produce smoother motions enable_action_filter: Boolean specifying if a lowpass filter should be used to smooth actions. g @d)maxlenrNTz!Must provide a motor model class!z@on_rack and reset_at_current_position cannot be enabled together)kpkd torque_limitsmotor_control_moderrrr)position orientationg) reset_time)8 num_motorsnum_legs_pybullet_client_action_repeat _urdf_root_self_collision_enabled_motor_direction _motor_offsetnpzeros_observed_motor_torques_applied_motor_torques _max_force _pd_latency_control_latency_observation_noise_stdev collectionsdeque_observation_history_control_observation_chassis_link_ids _leg_link_ids_motor_link_ids_foot_link_ids_motor_overheat_protection_on_rack_reset_at_current_position SetAllSensorslist safety_config_is_safe_safety_checker_enable_action_interpolation_enable_action_filter _last_action ValueError isinstanceSequencendarrayasarray _motor_kpsfull _motor_kds_motor_torque_limits_motor_control_mode _motor_model time_step _step_counter_state_action_counterinvertTransform_GetDefaultInitOrientation_init_orientation_inv_BuildActionFilter_action_filterResetReceiveObservation)selfpybullet_clientr? dofs_per_leg urdf_rootrm action_repeatself_collision_enabledr:motor_model_classmotor_kpmotor_kdmotor_torque_limits pd_latencycontrol_latencyobservation_noise_stdevmotor_overheat_protectionmotor_direction motor_offseton_rackreset_at_current_positionsensorsr\enable_action_interpolationenable_action_filter_s r.__init__Minitaur.__init__KsZ!OOO|3DM+'O#9 +D%D#%88DOO#?? 8,do 5do([112::>?? 8,do 5do% (<(HmdZ]11':;r0cFURRUR5nS/Ul/Ul/Ul/Ul/Ul[U5GHnURRURU5nUSRS5nURUn[RU5(aURRU5 M[RU5(aURRU5 M[ RU5(aUR RU5 M["RU5(aUR RU5 GM*[$RU5(d4[&RU5(d[(RU5(aURRU5 GM[+SU-5e URR-UR 5 URR/5 UR R/5 UR R/5 URR/5 URR/5 g)znBuild the link Ids from its name in the URDF file. Raises: ValueError: Unknown category of the joint name. rrrzUnknown category of joint %sN)rArrrSrTrUrV_bracket_link_idsr%rrr_CHASSIS_NAME_PATTERNmatchr_BRACKET_NAME_PATTERN_MOTOR_NAME_PATTERN_KNEE_NAME_PATTERN_LEG_NAME_PATTERN1_LEG_NAME_PATTERN2_LEG_NAME_PATTERN3rbrsort)rwrr-r joint_namejoint_ids r. _BuildUrdfIdsMinitaur._BuildUrdfIds3s &&33DNNCJ TDDDDD : ((55dnnaHja=''0j'' 3h  $ $Z 0 0 %%h/ & &z 2 2 %%h/  $ $Z 0 0 ##H-  # #J / / ""8,  $ $Z 0 0  $ $Z 0 0  $ $Z 0 0 !!(+7*DEE#& d112!!r0cURRUR5n[U5HHnURR URU5nURR USSSSS9 MJ g)Nrr) linearDampingangularDamping)rArrr%rchangeDynamicsrs r._RemoveDefaultJointDamping#Minitaur._RemoveDefaultJointDampingZsn&&33DNNCJ : ((55dnnaHj ** Q-1Q+@r0ctUR5Vs/sHnURUPM snUlgs snfr)_GetMotorNamesr_motor_id_list)rw motor_names r._BuildMotorIdListMinitaur._BuildMotorIdListas=--//J z*/Ds5c URRURSSSURR/SQ/SQUUS9 nU$)a=Create a constraint that keeps the chassis at a fixed frame. This frame is defined by init_position and init_orientation. Args: init_position: initial position of the fixed frame. init_orientation: initial orientation of the fixed frame in quaternion format [x,y,z,w]. Returns: Return the constraint id. rr;) parentBodyUniqueIdparentLinkIndexchildBodyUniqueIdchildLinkIndex jointType jointAxisparentFramePositionchildFramePositionchildFrameOrientation)rAcreateConstraintr JOINT_FIXED)rw init_positioninit_orientationfixed_constraints r._CreateRackConstraintMinitaur._CreateRackConstraintgsS,,==>>''33%(.> 0 r0cg)a<Whether the observation is valid for the current time step. In simulation, observations are always valid. In real hardware, it may not be valid from time to time when communication error happens between the Nvidia TX2 and the microcontroller. Returns: Whether the observation is valid for the current time step. Trrs r.IsObservationValidMinitaur.IsObservationValids r0c0U(aUR5 UR(a3URUR5UR 55UlUR 5 UR5 UR5 UR5 UR5 UR5 URSS9 O}URRURUR5UR 55 URR!UR/SQ/SQ5 URSS9 ["R$"UR&5UlS/UR&-UlUR,R/5 SUlSUlSUlSUlUR8(a[:R<"U5UlURAX#5 URB(aURE5 g)a<Reset the minitaur to its initial states. Args: reload_urdf: Whether to reload the urdf file. If not, Reset() just place the minitaur back to its starting position. default_motor_angles: The default motor angles. If it is None, minitaur will hold a default pose (motor angle math.pi / 2) for 100 steps. In torque control mode, the phase of holding the default pose is skipped. reset_time: The duration (in seconds) to hold the default motor angles. If reset_time <= 0 or in torque control mode, the phase of holding the default pose is skipped. T)add_constraintr;FrN)#_LoadRobotURDFrXr_GetDefaultInitPositionrqrack_constraintrrrrrr ResetPoserAresetBasePositionAndOrientationrresetBaseVelocityrGrHr?_overheat_counter_motor_enabled_listrQclearrnror]rar\r SafetyCheckerr^_SettleDownForResetr`_ResetActionFilter)rw reload_urdfdefault_motor_anglesr>s r.ruMinitaur.Resets    & &t'C'C'E'+'F'F'H J  ""$  %%'  ""$ %%' nnDn) ;; ..$668  ) ) +- --dnni.79 nnEn*XXdoo6D $v7D##%D!"DDMD +99$?d1> !!  r0cdUR5nUR(aRURRUUR 5UR 5URR S9UlgURRXR 5UR 55Ulg)z"Loads the URDF file for the robot.)flagsN) GetURDFFilerDrAloadURDFrrqURDF_USE_SELF_COLLISIONr)rw urdf_files r.rMinitaur._LoadRobotURDFs  "I ##,,55   & & (  ) ) +%%== 6?dn ,,55 113  ) ) +-dnr0cUS::agUR5 [S5H]nUR[RS- /UR -[ RRS9 UR(aM] g Ucg[X R- 5n[U5H>nURU[ RRS9 UR(aM> g g)a(Sets the default motor angles and waits for the robot to settle down. The reset is skipped is reset_time is less than zereo. Args: default_motor_angles: A list of motor angles that the robot will achieve at the end of the reset phase. reset_time: The time duration for the reset phase. rNr5r!)r:) rvr%rr'r*r?r MotorControlModePOSITIONr]intrm)rwr r>rnum_steps_to_resets r.rMinitaur._SettleDownForResetsQ   3Z  77Q;-$// ))::CCE]]] # Z..89 % &  )::CCE]]] 'r0cxURRURUURRUS9 g)N) bodyIndex jointIndex controlModeforce)rAsetJointMotorControl2rTORQUE_CONTROL)rwrtorques r._SetMotorTorqueByIdMinitaur._SetMotorTorqueByIds7//..))88 0r0cxURRURUURRUS9 g)N)r jointIndicesrforces)rAsetJointMotorControlArrayrr)rw motor_idstorquess r._SetMotorTorqueByIdsMinitaur._SetMotorTorqueByIdss733..))88 4r0cBURURUU5 gr)_SetDesiredMotorAngleByIdr)rwr desired_angles r._SetDesiredMotorAngleByName$Minitaur._SetDesiredMotorAngleByNames ""4#9#9*#E#02r0c SUR-$)Nz%s/quadruped/minitaur.urdf)rCrs r.rMinitaur.GetURDFFiles '$// 99r0c^[UR5HnURX!5 M g)zpReset the pose of the minitaur. Args: add_constraint: Whether to add a constraint at the joints of two feet. N)r%r@_ResetPoseForLeg)rwrr-s r.rMinitaur.ResetPoses% 4== ! A."r0c "Sn[RS- nSn[UnURR UR UR SU-S-URSU-U-SS9 URR UR UR SU-S -URSU-U-SS9 URR UR UR SU-S -URSU-S -U-SS9 URR UR UR SU-S -URSU-S -U-SS9 U(azURRUR UR SU-S -UR UR SU-S -URR/S Q[[5 URRUR UR SU-S-URRSUS9 URRUR UR SU-S -URRSUS9 URRUR UR SU-S -URRSUS9 URRUR UR SU-S -URRSUS9 g)zReset the initial pose for the leg. Args: leg_id: It should be 0, 1, 2, or 3, which represents the leg at front_left, back_left, front_right and back_right. add_constraint: Whether to add a constraint at the joints of two feet. rg@gPkwmotor_L_jointr!)targetVelocityknee_L_linkR_jointrR_linkr;rrrr8rN)r'r* LEG_POSITIONrAresetJointStaterrrErJOINT_POINT2POINTKNEE_CONSTRAINT_POINT_RIGHTKNEE_CONSTRAINT_POINT_LEFTrVELOCITY_CONTROL)rwrrknee_friction_forcehalf_pi knee_angle leg_positions r.r3Minitaur._ResetPoseForLegs1ggmGJ'L))  x,6BC a&j)G3 *  ))  w5@A a&j)J6 *  ))  x,6BC a&j1n-7 *  ))  w5@A a&j1n- : *  ,, ..  >r0c[UR5UR:XdeURUnURUR-n[ X-X-U-5Vs/sHofPM nn[ R "UUUUUS9n[R"[R"U5[R"UR5U- URU5nXxR54$s snf)z=Calculate the joint positions from the end effector position.)robot link_positionlink_id joint_idsposition_in_world_frame) r&rVr@r?r%r joint_angles_from_link_positionrGmultiplyrfrFrEtolist) rwrr<rjtoe_idmotors_per_legr-joint_position_idxs joint_angless r._EndEffectorIKMinitaur._EndEffectorIKs t"" #t}} 44 4   (F__ 5N0&2I'3())a)==% 7 9L;; <  4%%&':; < 124L  3 3 5 55#s# C5c"URXSS9$)a_Use IK to compute the motor angles, given the foot link's position. Args: leg_id: The leg index. foot_world_position: The foot link's position in the world frame. Returns: A tuple. The position indices and the angles for all joints along the leg. The position indices is consistent with the joint orders as returned by GetMotorAngles API. Trjrr)rwrfoot_world_positions r.'ComputeMotorAnglesFromFootWorldPosition0Minitaur.ComputeMotorAnglesFromFootWorldPositions#   T  CCr0c"URXSS9$)adUse IK to compute the motor angles, given the foot link's local position. Args: leg_id: The leg index. foot_local_position: The foot link's position in the base frame. Returns: A tuple. The position indices and the angles for all joints along the leg. The position indices is consistent with the joint orders as returned by GetMotorAngles API. Frurv)rwrfoot_local_positions r.'ComputeMotorAnglesFromFootLocalPosition0Minitaur.ComputeMotorAnglesFromFootLocalPositions#   U  DDr0c[UR5UR:Xde[R"UURUS9$)z%Compute the Jacobian for a given leg.rfrh)r&rVr@r compute_jacobian)rwrs r.ComputeJacobianMinitaur.ComputeJacobiansE t"" #t}} 44 4  & &##F+ r0cURU5n[R"X#5n0nURUR-nSn[ X-US-U-5HnUXx-UR U-XX'M U$)z5Maps the foot contact force to the leg joint torques.r)rrGmatmulr?r@r%rE) rwr contact_forcejvall_motor_torques motor_torquesrocom_dofrs r.MapContactForceToJointTorques&Minitaur.MapContactForceToJointTorquess  f %B -4M__ 5NG&1!A:79 1  ! $ 5 5h ?!@m9 r0c z/n[[S-5HnURUS-nURUS-S-n[URR SUR SUS95n[URR SUR SUS95nURU=(d U5 M U$)zGet minitaur's foot contact situation with the ground. Returns: A list of 4 booleans. The ith boolean is True if leg i is in contact with ground. r!rrr)bodyAbodyB linkIndexA linkIndexB)r%MINITAUR_NUM_MOTORSrVboolrAgetContactPointsrr)rwcontactsleg_idx link_id_1 link_id_2 contact_1 contact_2s r.GetFootContactsMinitaur.GetFootContactssH,12%%gk2i%%gkAo6i    0 0NN" 1 $%i     0 0NN" 1 $%i ooi,9-3 Or0c[UR5UR:Xde/nUR5H'nUR [ R "UUS95 M) [R"U5$z0Get the robot's foot position in the base frame.r) r&rVr@rrr link_position_in_world_framerGr[rwfoot_positionsfoot_ids r.GetFootPositionsInWorldFrame%Minitaur.GetFootPositionsInWorldFramesj t"" #t}} 44 4N&&(  1 1  ) 88N ##r0c[UR5UR:Xde/nUR5H'nUR [ R "UUS95 M) [R"U5$r) r&rVr@rrr link_position_in_base_framerGr[rs r.GetFootPositionsInBaseFrame$Minitaur.GetFootPositionsInBaseFramesj t"" #t}} 44 4N&&(  0 0  ) 88N ##r0cURVs/sHoSPM nn[R"[R"U5[R"UR5- UR 5nU$s snf)zwGets the eight motor angles at the current moment, mapped to [-pi, pi]. Returns: Motor angles, mapped to [-pi, pi]. r) _joint_statesrGrlrfrFrE)rwstate motor_angless r.GetTrueMotorAnglesMinitaur.GetTrueMotorAnglessf +/*<*<=*<!H*sA7cUR[R"URSUR5UR S5n[ U5$)zGets the eight motor angles. This function mimicks the noisy sensor reading and adds latency. The motor angles that are delayed, noise polluted, and mapped to [-pi, pi]. Returns: Motor angles polluted by noise and latency, mapped to [-pi, pi]. r)r\rGr[rRr?rNr/)rwrs r.GetMotorAnglesMinitaur.GetMotorAnglessL'' **1T__=> %%a(*L L ))r0cURVs/sHoSPM nn[R"X R5nU$s snf)zRGet the velocity of all eight motors. Returns: Velocities of all eight motors. r)rrGrlrE)rwrmotor_velocitiess r.GetTrueMotorVelocitiesMinitaur.GetTrueMotorVelocities!sE /3.@.@A.@Ua.@A{{#35J5JK BsAcUR[R"URURSUR-5UR S5$)zGet the velocity of all eight motors. This function mimicks the noisy sensor reading and adds latency. Returns: Velocities of all eight motors polluted by noise and latency. r!rr\rGr[rRr?rNrs r.GetMotorVelocitiesMinitaur.GetMotorVelocities,sR    **4??1+/??<;< = %%a( **r0cUR$)zaGet the amount of torque the motors are exerting. Returns: Motor torques of all eight motors. )rIrs r.GetTrueMotorTorquesMinitaur.GetTrueMotorTorques8s  ' ''r0cUR[R"URSUR-SUR-5UR S5$)zGet the amount of torque the motors are exerting. This function mimicks the noisy sensor reading and adds latency. Returns: Motor torques of all eight motors polluted by noise and latency. r!rYrrs r.GetMotorTorquesMinitaur.GetMotorTorques@sX    **1t+>q+/??@;< = %%a( **r0c[R"[R"UR5UR 555UR -UR -$)zGet the amount of energy used in last one time step. Returns: Energy Consumption based on motor velocities and torques (Nm^2/s). )rGabsdotrrrmrBrs r."GetEnergyConsumptionPerControlStep+Minitaur.GetEnergyConsumptionPerControlStepLsT 66"&&  !# $&*nn 57;7J7J KKr0cUR$)zsGet the orientation of minitaur's base, represented as quaternion. Returns: The orientation of minitaur's base. )_base_orientationrs r.rSMinitaur.GetTrueBaseOrientationVs  ! !!r0cTURRUR55$)zGet the orientation of minitaur's base, represented as quaternion. This function mimicks the noisy sensor reading and adds latency. Returns: The orientation of minitaur's base polluted by noise and latency. )rAgetQuaternionFromEulerr_rs r.GetBaseOrientationMinitaur.GetBaseOrientation^s)  7 7   " $$r0cURRUR5SnUR5nUR UU5$)zGet the rate of orientation change of the minitaur's base in euler angle. Returns: rate of (roll, pitch, yaw) change of the minitaur's base. r)rArNrrS$TransformAngularVelocityToLocalFrame)rwangular_velocityr=s r.GetTrueBaseRollPitchYawRate$Minitaur.GetTrueBaseRollPitchYawRatehsM ,,<+,,--.-@1-DF G %%a( **r0cUR$)zNGet the length of the action list. Returns: The length of the action list. )r?rs r.GetActionDimensionMinitaur.GetActionDimension ??r0cBUR(a[UR5Htn[X5[:aUR U==S- ss'OSUR U'UR U[ UR- :dMeSURU'Mv gg)NrrF) rWr%r?rOVERHEAT_SHUTDOWN_TORQUErOVERHEAT_SHUTDOWN_TIMErmr)rw actual_torquer-s r._ApplyOverheatProtection!Minitaur._ApplyOverheatProtections &&T__%! } #; ;   #q ( #&'$  #  " "1 % "T^^ 3 4(-$ " "1 %&'r0cURUR-UlUnUc URnUR(aURR X5 [R"U5nUR5upVUR5nURR!XXgU5upUR#U5 Xl[R&"UUR(5Ul/n /n [-UR.UR*UR05HSupnU(a$U R3U 5 U R3U 5 M1U R3U 5 U R3S5 MU UR5X5 g![ Ra(n[R"SU5 SUl SnAgSnAff=f)zApply the motor commands using the motor model. Args: motor_commands: np.array. Can be motor angles, torques, hybrid commands, or motor pwms (for Minitaur only). motor_control_mode: A MotorControlMode enum. NA safety error occurred: %sFr)rormlast_action_timerkr^check_motor_actionr SafetyErrorlogginginfor]rGrf_GetPDObservationrrlconvert_to_torquerrIrlrE_applied_motor_torqueziprrrr))rwmotor_commandsr: control_modeeqqdot qdot_truerobserved_torquer'rr motor_torque motor_enableds r.rMinitaur.ApplyActions!66GD%L--l  //M ZZ/N$$&GA++-I%)%6%6%H%H4L&:"M !!-0$3 "$]-1-B-B"DDIM14T5H5H595O5O595M5M2O-  "\*"Q2O i7C % % 2A6 sF GGGc<[R"U5nSnSnURS-n[RS- n[ UR5HFnUS-nU*U-XU-U--n SU-U-X-n Xu:aU *n [RU -U -X''MH U$)zConvert the actions that use leg model to the real motor actions. Args: actions: The theta, phi of the leg model. Returns: The eight desired motor angles that can be used in ApplyActions(). rg?r!rZr)r#r$r?r'r*r%) rwactions motor_anglescale_for_singularityoffset_for_singularityhalf_num_motors quater_pir- action_idxforward_backward_componentextension_components r.ConvertFromLegModelMinitaur.ConvertFromLegModels--(K oo*O! I 4?? #6j 9 , / 03I I K! !Gi/'2EE 22 ''. .1D Dn$ r0cUR$)z,Get the mass of the base from the URDF file.)rrs r.GetBaseMassesFromURDFMinitaur.GetBaseMassesFromURDFs   r0cUR$)z/Get the inertia of the base from the URDF file.)rrs r.GetBaseInertiasFromURDF Minitaur.GetBaseInertiasFromURDFs  " ""r0cUR$)z,Get the mass of the legs from the URDF file.)rrs r.GetLegMassesFromURDFMinitaur.GetLegMassesFromURDF   r0cUR$)z/Get the inertia of the legs from the URDF file.)rrs r.GetLegInertiasFromURDFMinitaur.GetLegInertiasFromURDF  ! !!r0c<[U5[UR5:wa7[SR[U5[UR555e[ URU5H)up#UR R URX#S9 M+ g)a;Set the mass of minitaur's base. Args: base_mass: A list of masses of each body link in CHASIS_LINK_IDS. The length of this list should be the same as the length of CHASIS_LINK_IDS. Raises: ValueError: It is raised when the length of base_mass is not the same as the length of self._chassis_link_ids. zJThe length of base_mass {} and self._chassis_link_ids {} are not the same.massN)r&rSrbformatrrArr)rw base_massr chassis_masss r. SetBaseMassesMinitaur.SetBaseMassess 9~T3344  fS^S1G1G-HI KK%((>(> $J  ** ..*+9%Kr0c[U5[UR5[UR5-:wa [S5e[ URU5H)up#UR R URX#S9 M+ U[UR5Sn[ URU5H)upVUR R URXVS9 M+ g)aSet the mass of the legs. A leg includes leg_link and motor. 4 legs contain 16 links (4 links each) and 8 motors. First 16 numbers correspond to link masses, last 8 correspond to motor masses (24 total). Args: leg_masses: The leg and motor masses for all the leg links and motors. Raises: ValueError: It is raised when the length of masses is not equal to number of links + motors. ^The number of values passed to SetLegMasses are different than number of leg links and motors.rN)r&rTrUrbrrArr)rw leg_massesrleg_mass motor_massesrh motor_masss r. SetLegMassesMinitaur.SetLegMassess :#d001C8L8L4MMM H II 2 2J? ** ..&+1@c$"4"4567L"4#7#7F ** ..'+4 Gr0c[U5[UR5:wa7[SR[U5[UR555e[ URU5Hiup#UH:n[ R "U5S:R5(dM1[S5e URRURX#S9 Mk g)a|Set the inertias of minitaur's base. Args: base_inertias: A list of inertias of each body link in CHASIS_LINK_IDS. The length of this list should be the same as the length of CHASIS_LINK_IDS. Raises: ValueError: It is raised when the length of base_inertias is not the same as the length of self._chassis_link_ids and base_inertias contains negative values. zNThe length of base_inertias {} and self._chassis_link_ids {} are not the same.r0Values in inertia matrix should be non-negative.localInertiaDiagonalN) r&rSrbrrrGrfanyrArr)rw base_inertiasrchassis_inertia inertia_values r.SetBaseInertiasMinitaur.SetBaseInertias1s =S!7!788   &- #d&<&<"=? @@(+4+A+A+8(:# *- JJ} % ) . . 0 0MN N+ ** ..*+L (:r0c[U5[UR5[UR5-:wa [S5e[ URU5Hiup#UH:n[ R "U5S:R5(dM1[S5e URRURX#S9 Mk U[UR5Sn[ URU5HiupgUH:n[ R "U5S:R5(dM1[S5e URRURXgS9 Mk g)aSet the inertias of the legs. A leg includes leg_link and motor. 4 legs contain 16 links (4 links each) and 8 motors. First 16 numbers correspond to link inertia, last 8 correspond to motor inertia (24 total). Args: leg_inertias: The leg and motor inertias for all the leg links and motors. Raises: ValueError: It is raised when the length of inertias is not equal to the number of links + motors or leg_inertias contains negative values. rrr$r%N) r&rTrUrbrrGrfr'rArr)rw leg_inertiasr leg_inertiar*motor_inertiasrh motor_inertias r.SetLegInertiasMinitaur.SetLegInertiasKsA <C 2 23c$:N:N6OOO H II"4#5#5|D'- JJ} % ) . . 0 0MN N( ** ..&+D E"#d&8&8"9":;N"%d&:&:N"K)- JJ} % ) . . 0 0MN N* ** ..'+G #Lr0crURH'nURRURX!S9 M) g)zSet the lateral friction of the feet. Args: foot_friction: The lateral friction coefficient of the foot. This value is shared by all four feet. )lateralFrictionNrVrArr)rw foot_frictionrhs r.SetFootFrictionMinitaur.SetFootFrictionls7&& ** ..'+B'r0crURH'nURRURX!S9 M) g)zSet the coefficient of restitution at the feet. Args: foot_restitution: The coefficient of restitution (bounciness) of the feet. This value is shared by all four feet. ) restitutionNr6)rwfoot_restitutionrhs r.SetFootRestitutionMinitaur.SetFootRestitutionxs7&& ** ..'+A'r0c [URU5H@up#URRURUURR SUS9 MB g)Nrr=)rrVrArrrC)rwjoint_frictions knee_joint_idfrictions r.SetJointFrictionMinitaur.SetJointFrictionsU#&t':':O#L  11NN"++<< 2$Mr0c,[UR5$r)r&rVrs r.GetNumKneeJointsMinitaur.GetNumKneeJointss t"" ##r0c:URRU5 gr)rl set_voltage)rwvoltages r.SetBatteryVoltageMinitaur.SetBatteryVoltages!!'*r0c:URRU5 gr)rlset_viscous_damping)rwviscous_dampings r.SetMotorViscousDampingMinitaur.SetMotorViscousDampings))/:r0c@/nURUR55 URUR55 URUR55 URUR 55 URUR 55 U$r)rrrrrSr)rw observations r.GetTrueObservationMinitaur.GetTrueObservationsKt..01t2245t//12t2245t779: r0cURRURUR5UlURR UR5uUlnURR/SQU/SQURS9uo l URRUR55 UR5UlURUR -UlUR$(aUR$R'5 gg![(R*a(n[,R."SU5 SUlSnAgSnAff=f)zReceive the observation from sensors. This function is called once per step. The observations are only updated when this function is called. r;) positionA orientationA positionB orientationBrFN)rAgetJointStatesrrrgetBasePositionAndOrientationrJrrrrrQ appendleftrT_GetControlObservationrRrormlast_state_timer^ check_stater rrrr])rwr=rrs r.rvMinitaur.ReceiveObservations$ ..== ++-D ;;DNNK%D!% 5 5 H H // !I!1A  (()@)@)BC $ ; ; =D55FD  ((* % % 2A6 sDE2EEcUS::d[UR5S:XaURSnU$[XR- 5nUS-[UR5:aURS$XUR-- nX@R- nSU- [R "URU5-U[R "URUS-5--nU$)zGet observation that is delayed by the amount specified in latency. Args: latency: The latency (in seconds) of the delayed observation. Returns: observation: The observation which was actually latency seconds ago. rrrr)r&rQrrmrGr[)rwlatencyrS n_steps_agoremaining_latency blend_alphas r._GetDelayedObservationMinitaur._GetDelayedObservations!|s4445:--a0k ..01k qC 9 9: :((,,!$..$@@%6k  )B)B;)O P P "((4#<#<[1_#MN N O r0cURUR5nUSURnXRSUR-n[R"U5[R"U54$)Nrr!)rgrLr?rGr[)rwpd_delayed_observationrrs r.rMinitaur._GetPDObservations[!889I9IJq1A !//!doo2E FD HHQK$ ((r0c<URUR5nU$r)rgrM)rwcontrol_delayed_observations r.r^Minitaur._GetControlObservations""&"="= # &&r0cjUS::aU$U[RRX!RS9-nU$)Nr)scalesize)rGrandomnormalshape)rw sensor_values noise_stdevrSs r.r\Minitaur._AddSensorNoises?a "))"2"2 3 3#3#55K r0cXlg)zSet the latency of the control loop. It measures the duration between sending an action from Nvidia TX2 and receiving the observation from microcontroller. Args: latency: The latency (in seconds) of the control loop. NrM)rwrcs r.SetControlLatencyMinitaur.SetControlLatencys $r0cUR$)zGet the control latency. Returns: The latency (in seconds) between when the motor command is sent and when the sensor measurements are reported back to the controller. ryrs r.GetControlLatencyMinitaur.GetControlLatencys   r0c[U[R[R45(a[R "U5UlO&[R"URU5Ul[U[R[R45(a[R "U5Ul O&[R"URU5Ul URRX5 g)zSet the gains of all motors. These gains are PD gains for motor positional control. kp is the proportional gain and kd is the derivative gain. Args: kp: proportional gain(s) of the motors. kd: derivative gain(s) of the motors. N) rcrOrdrGrerfrgrhr?rirlset_motor_gains)rwr7r8s r. SetMotorGainsMinitaur.SetMotorGainss"{++RZZ899 2do4do"{++RZZ899 2do4do%%b-r0c2URUR4$)zVGet the gains of the motor. Returns: The proportional gain. The derivative gain. )rgrirs r. GetMotorGainsMinitaur.GetMotorGains s ??DOO ++r0cUR$)zHGet the position gains of the motor. Returns: The proportional gain. )rgrs r.GetMotorPositionGainsMinitaur.GetMotorPositionGainsrr0cUR$)zFGet the velocity gains of the motor. Returns: The derivative gain. )rirs r.GetMotorVelocityGainsMinitaur.GetMotorVelocityGainsrr0cVURRU/UR-5 g)zSet the strength of all motors relative to the default value. Args: ratio: The relative strength. A scalar range from 0.0 to 1.0. N)rlset_strength_ratiosr?)rwratios r.SetMotorStrengthRatioMinitaur.SetMotorStrengthRatio&s#  ))5'DOO*CDr0c:URRU5 g)zSet the strength of each motor relative to the default value. Args: ratios: The relative strength. A numpy array ranging from 0.0 to 1.0. N)rlr)rwratioss r.SetMotorStrengthRatiosMinitaur.SetMotorStrengthRatios.s  ))&1r0cX lXlg)zSet the time steps of the control and simulation. Args: action_repeat: The number of simulation steps that the same action is repeated. simulation_step: The simulation time step. N)rmrB)rwr{simulation_steps r. SetTimeStepsMinitaur.SetTimeSteps6s%N'r0c[$r) MOTOR_NAMESrs r.rMinitaur._GetMotorNamesAs r0cUR(a1UR(a UR5upn[u p4XU/$UR(a[ $[$)zReturns the init position of the robot. It can be either 1) origin (INIT_POSITION), 2) origin with a rack (INIT_RACK_POSITION), or 3) the previous position. )rYrQrK INIT_POSITIONrXINIT_RACK_POSITION)rwxyrzs r.r Minitaur._GetDefaultInitPositionDsM &&4+D+D$$&gaAgaAY }}  r0cUR(aBUR(a1UR5u pURR SSU/5$[ $)zrReturns the init position of the robot. It can be either 1) INIT_ORIENTATION or 2) the previous rotation in yaw. r)rYrQr_rArINIT_ORIENTATION)rwryaws r.rq#Minitaur._GetDefaultInitOrientationUsJ  &&4+D+D**,ia  " " 9 93S/ JJ r0cUR$r)rSrs r.chassis_link_idsMinitaur.chassis_link_ids`rr0cDUHnURU5 M Xlg)zwset all sensors to this robot and move the ownership to this robot. Args: sensors: a list of sensors to this robot. N) set_robot_sensors)rwrss r.rZMinitaur.SetAllSensorsds kk$Mr0cUR$)zXget all sensors associated with this robot. Returns: sensors: a list of all sensors. )rrs r. GetAllSensorsMinitaur.GetAllSensorsns ==r0cZURHnUR5U:XdMUs $ g)zget the first sensor with the given name. This function return None if a sensor with the given name does not exist. Args: name: the name of the sensor we are looking Returns: sensor: a sensor with the given name. None if not exists. N)rget_name)rwnamers r. GetSensorMinitaur.GetSensorvs*]]   r0cUR$r)r]rs r.is_safeMinitaur.is_safes ==r0cUR$r)rars r. last_actionMinitaur.last_actions   r0cUR(aPURb URnOUR5n[US-5UR- nX4X- --nU$UnU$)a(If enabled, interpolates between the current and previous actions. Args: action: current action. substep_count: the step count should be between [0, self.__action_repeat). Returns: If interpolation is enabled, returns interpolated action depending on the current action repeat substep. r)r_rarfloatrB)rwr substep_count prev_actionlerprs r.rMinitaur.ProcessActionsq ((   &'' ))+ =1$ %(;(; ;d&*>"??k k r0cSURUR-- nUR5n[R"XS9nU$)Nr) sampling_rater)rmrBrr ActionFilterButter)rwrra_filters r.rsMinitaur._BuildActionFiltersA$*=*==>M((*J//#rcompilerrrrrrrrrrrr*r)rrr/ configurableobjectr2rr0r.rs7L&%   <86?=A> -+G   #67jj!12ZZ + #<=ZZ/ZZ .ZZ0/$@!!9 TWW H &vr0