nifSrSSKrSSKr\RR \RR \R "\R"5555r\RR \RR \55r \RRRS\ 5 SSK r SSK r SSKrSSKrSSKrSSKJr /SQr/SQr/SQr/SQr/SQrS rS r/S Qr/S Qr\R:"S 5r\R:"S5r\R:"S5r Sr!S\RD-r#Sr$"SS\%5r&g)zHThis file implements the functionalities of a minitaur using pybullet. N)motor)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*)rrrrcH[R"U5n[[U55Hsn[R "X[ 5X'X[R:aX==[ -ss'MJX[R*:dMcX==[ - ss'Mu U$)z~Maps 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/envs/minitaur.pyMapToMinusPiToPir#&s~--'- V ayyF3M477"&   TWWH $&   c\rSrSrSrSSSS\R SSSSSS S \SSS4S jrS r S r S r Sr Sr SrSrSrSrSrSISjrSrSrSrSrSrSrSrSrSrS rS!rS"rS#r S$r!S%r"S&r#S'r$S(r%S)r&SJS*jr'S+r(S,r)S-r*S.r+S/r,S0r-S1r.S2r/S3r0S4r1S5r2S6r3S7r4S8r5S9r6S:r7S;r8S<r9S=r:S>r;S?rSBr?SCr@SDrASErBSFrC\DSG5rESHrFg)KMinitaur8zLThe minitaur class that simulates a quadruped robot from Ghost Robotics. rrFrg{Gz?rchSUl[URS- 5UlXlX@lX lXPlX`lXpl/SQUl [R"UR5Ul [R"UR5Ul SUlXlXlXlXlXl[(R*"SS9Ul/UlS/Ul/Ul/Ul/UlXlUUlUUlUR$(aFXlXl [BRD"UR8UR>UR@S9Ul#O.UR(aSUlS Ul OS UlS Ul X0l$S Ul%URMS S 9 g)aConstructs a minitaur and reset it to the initial states. Args: pybullet_client: The instance of BulletClient to manage different simulations. 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_velocity_limit: The upper limit of the motor velocity. pd_control_enabled: Whether to use PD control for the motors. accurate_motor_model_enabled: Whether to use the accurate DC motor model. remove_default_joint_damping: Whether to remove the default joint damping. motor_kp: proportional gain for the accurate motor model. motor_kd: derivative gain for the accurate motor model. 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] torque_control_enabled: Whether to use the torque control, if set to False, pose control will be used. 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. 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. r)r+r+r+rrrrg @d)maxlenr+)torque_control_enabledkpkdg333333?rrg) reset_timeN)' num_motorsintnum_legs_pybullet_client_action_repeat _urdf_root_self_collision_enabled_motor_velocity_limit_pd_control_enabled_motor_directionnpzeros_observed_motor_torques_applied_motor_torques _max_force _pd_latency_control_latency_observation_noise_stdev_accurate_motor_model_enabled_remove_default_joint_damping collectionsdeque_observation_history_control_observation_chassis_link_ids _leg_link_ids_motor_link_ids_foot_link_ids_torque_control_enabled_motor_overheat_protection_on_rack_kp_kdr MotorModel _motor_model time_step _step_counterReset)selfpybullet_client urdf_rootrU action_repeatself_collision_enabledmotor_velocity_limitpd_control_enabledaccurate_motor_model_enabledremove_default_joint_dampingmotor_kpmotor_kd pd_latencycontrol_latencyobservation_noise_stdevr.motor_overheat_protectionon_racks r"__init__Minitaur.__init__=smjDO!+,DM+'O#9 !518D#%88DOO#HmdZ]11':;r$cURRUR5nS/Ul/Ul/Ul/Ul[U5GH nURRURU5nUSRS5nURUn[RU5(aURRU5 M[RU5(aUR RU5 M[RU5(aUR RU5 MURRU5 GM URR!UR 5 URR#5 UR R#5 UR R#5 URR#5 g)z2Build the link Ids from its name in the URDF file.r+rrN)r5rrrJrKrLrMrrrr_CHASSIS_NAME_PATTERNmatchr}_MOTOR_NAME_PATTERN_KNEE_NAME_PATTERNrsort)rXrr!r joint_namejoint_ids r" _BuildUrdfIdsMinitaur._BuildUrdfIdsst&&33DNNCJ TDDDD : ((55dnnaHja=''0j'' 3h  $ $Z 0 0 %%h/  $ $Z 0 0 ##H-  # #J / / ""8, !!(+ d112!r$cURRUR5n[U5HHnURR URU5nURR USSSSS9 MJ g)Nrr+) linearDampingangularDamping)r5rrrrchangeDynamicsrs r"_RemoveDefaultJointDamping#Minitaur._RemoveDefaultJointDampingsi&&33DNNCJ : ((55dnnaHj **:a="A^_*`r$c^[Vs/sHoRUPM snUlgs snfrk) MOTOR_NAMESr_motor_id_list)rX motor_names r"_BuildMotorIdListMinitaur._BuildMotorIdLists'P[\P[*11*=P[\D\s*cg)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. Trxrls r"IsObservationValidMinitaur.IsObservationValids r$NcUR(a[nO[nU(GaUR(aMURR SUR -UURURRS9UlO7URR SUR -UURS9UlUR5 UR5 UR(aUR5 UR5 UR5 UR5 UR!SS9 OeURR#URU[$5 URR'UR/SQ/SQ5 UR!SS9 [(R*"UR,5UlS/UR,-UlSUlUR4R75 UR8(dUS :aUR;5 [=S 5H]nUR?[@RBS - /UR,-5 URRE5 UR;5 M_ Ubd[GX0RH- 5n[=U5H>nUR?U5 URRE5 UR;5 M@ UR;5 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. z%s/quadruped/minitaur.urdf) useFixedBaseflags)rT)add_constraintrrrFrrr,rN)%rPINIT_RACK_POSITION INIT_POSITIONr8r5loadURDFr7URDF_USE_SELF_COLLISIONrrrrErrrr ResetPoseresetBasePositionAndOrientationINIT_ORIENTATIONresetBaseVelocityr<r=r2_overheat_counter_motor_enabled_listrVrHclearrNrrrrprrrqr3rU)rX reload_urdfdefault_motor_anglesr1 init_positionrtnum_steps_to_resets r"rWMinitaur.Resets[ }}(m#m % %..77 (4?? : ''?? 8A ..778T8<9H8EEI]]8T ""$  + + '')  ""$ %%' nnDn) ;;DNNMs6//$..;C<@r$c URRURUURRUURUR UR S9 g)N)rrrtargetPosition positionGain velocityGainr)r5rrPOSITION_CONTROLrQrRr@)rXr desired_angles r"_SetDesiredMotorAngleById"Minitaur._SetDesiredMotorAngleByIdDsL//$..;C<@Y $ &  ))T-E-E 11NN,,X -Dy-PQ++<<# 2%  11NN,,X -Dy-PQ++<<# 2% &&x,'>'J'+'<''J'+'<'rr5rrr<rr;)rXr motor_torquess r"GetTrueMotorTorquesMinitaur.GetTrueMotorTorquess  ))T-E-E  ) ))---h    - -dnnh G J-kk-1F1FGm  s3BcUR[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. rrrrls r"GetMotorTorquesMinitaur.GetMotorTorquessP    **1t+>q4???RST %%a( **r$cTURRUR5upU$)zsGet the orientation of minitaur's base, represented as quaternion. Returns: The orientation of minitaur's base. r)rXrtrs r"rMinitaur.GetTrueBaseOrientations& ++II$..YNA r$cTURRUR55$)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. )r5getQuaternionFromEulerr rls r"GetBaseOrientationMinitaur.GetBaseOrientations$  7 78P8P8R SSr$cURRUR5n[R"USSUSSUSS/5$)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. rrr)r5getBaseVelocityrr<r)rXvels r"GetTrueBaseRollPitchYawRate$Minitaur.GetTrueBaseRollPitchYawRate$sL    / / ?C ::s1vay#a&)SVAY7 88r$cUR[R"URSUR-S-SUR-S-5UR S5$)zGet the rate of orientation change of the minitaur's base in euler angle. This function mimicks the noisy sensor reading and adds latency. Returns: rate of (roll, pitch, yaw) change of the minitaur's base polluted by noise and latency. rrrrls r"GetBaseRollPitchYawRate Minitaur.GetBaseRollPitchYawRate-s[    **1t+>+B1tCVYZCZ[\ %%a( **r$cUR$)zNGet the length of the action list. Returns: The length of the action list. )r2rls r"GetActionDimensionMinitaur.GetActionDimension9s ??r$crUR[R:a]UR5nX@RUR--nX@RUR-- n[R "XU5nUc![R "SUR5nUc![R "SUR5nUR(dUR(GaUR5upxUR5n UR(GaJURRXXX#5upUR(a[!UR"5Htn [%X5[&:aUR(U ==S- ss'OSUR(U 'UR(U [*UR- :dMeSUR,U 'Mv Xl[R0"XR25Ul[7UR8UR4UR,5H2upnU(aUR;X5 M UR;U S5 M4 gSU-Xq- -X8-- nUUl[R0"UR.UR25Ul[7UR8UR<5HupUR;X5 M g[R0"XR25n[7UR8U5Hun nUR?U U5 M g)aSet the desired motor angles to the motors of the minitaur. The desired motor angles are clipped based on the maximum allowed velocity. If the pd_control_enabled is True, a torque is calculated according to the difference between current and desired joint angle, as well as the joint velocity. This torque is exerted to the motor. For more information about PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller. Args: motor_commands: The eight desired motor angles. motor_kps: Proportional gains for the motor model. If not provided, it uses the default kp of the minitaur for all the motors. motor_kds: Derivative gains for the motor model. If not provided, it uses the default kd of the minitaur for all the motors. Nr*rrFr+) r9r<infrrUclipfullrQrRrDr:_GetPDObservationrrTconvert_to_torquerOrr2absOVERHEAT_SHUTDOWN_TORQUErOVERHEAT_SHUTDOWN_TIMErr>rr;_applied_motor_torqueziprrr?r)rXmotor_commands motor_kps motor_kdscurrent_motor_anglemotor_commands_maxmotor_commands_minqqdot qdot_true actual_torqueobserved_torquer!r motor_torque motor_enabledtorque_commandsmotor_commands_with_directionmotor_command_with_directions r"rpMinitaur.ApplyActionAs  !!BFF* 335/..4C]C]2]]/..4C]C]2]]ww~CUVn''!TXX&i''!TXX&i ))T-E-E-E&&(ga--/i + + +)-):):)L)L t *F&  * *)a=#$'??$$Q'1,'*+d$$Q'&&q),BT^^,SS,1d&&q) *(7$&([[@U@U%V"589L9L9=9S9S9=9Q9Q6S 1HM  $ $X <  $ $Xq 1 6Sy.A,>?)BRR(7$')kk$2N2N262G2G'I#'*$*=*=t?Z?Z&[ "H  " "8 :'\')kk.BWBW&X#478K8K8U5W 0(0 &&x1MN5Wr$c`[R"U5nSnSn[URS- 5n[R S- n[ UR5HOn[US-5nU*U-XU-U--n SU-U-X-n Xu:aU *n [R U -U -X''MQ 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?rrr+)rrr3r2rrr) rXactions motor_anglescale_for_singularityoffset_for_singularityhalf_num_motors quater_pir! action_idxforward_backward_componentextension_components r"ConvertFromLegModelMinitaur.ConvertFromLegModels--(K $//A-.O! I 4?? #qAv;j 9 , / 03I I K! !Gi/'2EE 22"<T>T:U#V XX$'(>(> $J  **4>>:*Y%Kr$c[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.roN)rrKrLrqrCr5rr)rX leg_massesrleg_mass motor_masseslink_id motor_masss r" SetLegMassesMinitaur.SetLegMassess :#d001C8L8L4MMM H II 2 2J? **4>>6*Q@c$"4"4567L"4#7#7F **4>>7*T Gr$c[U5[UR5:wa7[SR[U5[UR555e[ URU5Hjup#UH:n[ R "U5S:R5(dM1[S5e URRURUUS9 Ml 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) rrJrqrrrCr<ranyr5rr)rX base_inertiasrchassis_inertia inertia_values r"SetBaseInertiasMinitaur.SetBaseInertiass =S!7!788 ''-vc-.@#dF\F\B]'^ ``'*4+A+A='Q# *- JJ} % ) . . 0 0MN N+ **4>>+5@O+Q (Rr$c[U5[UR5[UR5-:wa [S5e[ URU5Hjup#UH:n[ R "U5S:R5(dM1[S5e URRURUUS9 Ml U[UR5Sn[ URU5HjupgUH:n[ R "U5S:R5(dM1[S5e URRURUUS9 Ml 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. rxrrrN) rrKrLrqrCr<rrr5rr)rX leg_inertiasr leg_inertiarmotor_inertiasr| motor_inertias r"SetLegInertiasMinitaur.SetLegInertiassG <C 2 23c$:N:N6OOO H II"4#5#5|D'- JJ} % ) . . 0 0MN N( **4>>+1@K+M E"#d&8&8"9":;N"%d&:&:N"K)- JJ} % ) . . 0 0MN N* **4>>+2@M+O #Lr$crURH'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. )lateralFrictionNrMr5rr)rX foot_frictionr|s r"SetFootFrictionMinitaur.SetFootFrictions1&& **4>>7*b'r$crURH'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. ) restitutionNr)rXfoot_restitutionr|s r"SetFootRestitutionMinitaur.SetFootRestitution s1&& **4>>7*a'r$c [URU5H@up#URRURUURR SUS9 MB g)Nrr)rCrMr5rrr)rXjoint_frictions knee_joint_idfrictions r"SetJointFrictionMinitaur.SetJointFriction*sU#&t':':O#L  11NN"++<< 2$Mr$c,[UR5$rk)rrMrls r"GetNumKneeJointsMinitaur.GetNumKneeJoints3s t"" ##r$c^UR(aURRU5 ggrk)rDrT set_voltage)rXvoltages r"SetBatteryVoltageMinitaur.SetBatteryVoltage6s$ )) ##G,*r$c^UR(aURRU5 ggrk)rDrTset_viscous_damping)rXviscous_dampings r"SetMotorViscousDampingMinitaur.SetMotorViscousDamping:s$ )) ++O<*r$c@/nURUR55 URUR55 URUR55 URUR 55 URUR 55 U$rk)rrrr"rr0)rX observations r"GetTrueObservationMinitaur.GetTrueObservation>sKt..01t2245t//12t2245t779: r$cURRUR55 UR5Ulg)zReceive the observation from sensors. This function is called once per step. The observations are only updated when this function is called. N)rH appendleftr_GetControlObservationrIrls r"rrMinitaur.ReceiveObservationGs2  (()@)@)BC $ ; ; =Dr$cUS::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. rrr+r)rrHr3rUr<r)rXlatencyr n_steps_agoremaining_latency blend_alphas r"_GetDelayedObservationMinitaur._GetDelayedObservationPs!|s4445:--a0k ..01k qC 9 9: :((,,!$..$@@%6kK'288D4M4Mk4Z+[[ 288D,E,EkTUo,V#WWXk r$cURUR5nUSURnXRSUR-n[R"U5[R"U54$)Nrr)rrAr2r<r)rXpd_delayed_observationrJrKs r"r=Minitaur._GetPDObservationds[!889I9IJq1A !//!doo2E FD HHQK$ ((r$c<URUR5nU$rk)rrB)rXcontrol_delayed_observations r"rMinitaur._GetControlObservationjs"&"="=d>S>S"T &&r$cjUS::aU$U[RRX!RS9-nU$)Nr)scalesize)r<randomnormalshape)rX sensor_values noise_stdevrs r"rMinitaur._AddSensorNoisens8a "))"2"2K^K^"2"__K r$cXlg)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. NrB)rXrs r"SetControlLatencyMinitaur.SetControlLatencyts $r$cUR$)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. rrls r"GetControlLatencyMinitaur.GetControlLatencys   r$cvXlX lUR(aURR X5 gg)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 of the motors. kd: derivative gain of the motors. N)rQrRrDrTset_motor_gains)rXr/r0s r" SetMotorGainsMinitaur.SetMotorGainss0HH )) ''/*r$c2URUR4$)zVGet the gains of the motor. Returns: The proportional gain. The derivative gain. )rQrRrls r" GetMotorGainsMinitaur.GetMotorGainss 88TXX r$czUR(a*URRU/UR-5 gg)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)rDrTset_strength_ratiosr2)rXratios r"SetMotorStrengthRatioMinitaur.SetMotorStrengthRatios1  )) ++UGdoo,EF*r$c^UR(aURRU5 gg)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)rDrTr)rXratioss r"SetMotorStrengthRatiosMinitaur.SetMotorStrengthRatioss&  )) ++F3*r$cX 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)rUr6)rXr[simulation_steps r" SetTimeStepsMinitaur.SetTimeStepss%N'r$cUR$rk)rJrls r"chassis_link_idsMinitaur.chassis_link_idsrmr$)*rDr6rBr?rr|rJrBrIrMrrRrQrrKrrr@r;rrrLrTrOr9rHrCr>rPrr:rAr5rEr8rVrNr7r4r2rrU)TNg@)NN)G__name__ __module__ __qualname____firstlineno____doc__r<r:SENSOR_NOISE_STDDEVrhrmruryrrrrrrrrWrrrrrrrr rrrrr"r%rr+r0r4r7rpr_rbrerhrkrur~rrrrrrrrrrrrr=rrrrrrrrrpropertyr__static_attributes__rxr$r"r&r&8s &+$&FF"',1,1"':&+).#_ B/  NN L4a ] >@> GV/H#T&   *  * *T9 *IOV0 #!"Z U.Q.!OFcb$-=>() ' $! 0G4 ( " "r$r&)'rosinspectpathdirnameabspathgetfile currentframe currentdir parentdirsysinsertrFrrrenumpyr<pybullet_envs.minitaur.envsrrrrrrr@rArrcompilerrrrrrr#objectr&rxr$r"rs WW__RWW__W__W=Q=Q=S-TU V GGOOBGGOOJ7 8  1i  - -+G   #67jj!12ZZ +/ TWW$G"vG"r$