niXfSrSSKrSSKJr SrSrSrSrSr Sr \\ \ -- r S r S r S r"S S \5rg)z-This file implements an accurate motor model.N) robot_config2g@g0@g+?g h"l?g?g@c\rSrSrSrSSS\R R4SjrSr Sr S r S r S r S rSS jrSrSrg) MotorModelaThe accurate motor model, which is based on the physics of DC motors. The motor model support two types of control: position control and torque control. In position control mode, a desired motor angle is specified, and a torque is computed based on the internal motor model. When the torque control is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the torque. The internal motor model takes the following factors into consideration: pd gains, viscous friction, back-EMF voltage and current-torque profile. g333333?rNcXlX lX0lX@l[Ul[ Ul[Ul [Ul /SQUl /SQUl S/[-Ulg)N)r (r<)rgffffff?g@g@g @g @?)_kp_kd_torque_limits_motor_control_modeMOTOR_RESISTANCE _resistance MOTOR_VOLTAGE_voltageMOTOR_TORQUE_CONSTANT_torque_constantMOTOR_VISCOUS_DAMPING_viscous_damping_current_table _torque_table NUM_MOTORS_strength_ratios)selfkpkd torque_limitsmotor_control_modes f/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/minitaur_motor.py__init__MotorModel.__init__"sX HH'1'D!DM1D1D5D:D EJ.Dc:[R"U5Ulg)zSet the strength of each motors relative to the default value. Args: ratios: The relative strength of motor output. A numpy array ranging from 0.0 to 1.0. N)nparrayr )r!ratioss r&set_strength_ratiosMotorModel.set_strength_ratios3sHHV,Dr)cXlX lg)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)rr)r!r"r#s r&set_motor_gainsMotorModel.set_motor_gains<sHHr)cXlgNr)r!voltages r& set_voltageMotorModel.set_voltageIsMr)cUR$r4r5r!s r& get_voltageMotorModel.get_voltageLs ==r)cXlgr4r)r!viscous_dampings r&set_viscous_dampingMotorModel.set_viscous_dampingOs+r)cUR$r4r>r:s r&get_viscous_damplingMotorModel.get_viscous_damplingRs   r)cpU(d URnU[RRLdU[RRLa[ SR U55eURnURnU[RRLacSU-U[- -US- U-- nSU-U[- -US- U-- n U[R"US5-[R"U S5-n OSU-X!- -Xs-- n [R"U SS5n UR!X5$)a=Convert the commands (position control or pwm control) to torque. Args: motor_commands: The desired motor angle if the motor is in position control mode. The pwm signal if the motor is in torque control mode. motor_angle: The motor angle observed at the current time step. It is actually the true motor angle observed a few milliseconds ago (pd latency). motor_velocity: The motor velocity observed at the current time step, it is actually the true motor velocity a few milliseconds ago (pd latency). true_motor_velocity: The true motor velocity. The true velocity is used to compute back EMF voltage and viscous damping. motor_control_mode: A MotorControlMode enum. Returns: actual_torque: The torque that needs to be applied to the motor. observed_torque: The torque observed by the sensor. z({} is not a supported motor control modeg@rr)rrMotorControlModeTORQUEHYBRID ValueErrorformatrrPWM MOTOR_POS_UB MOTOR_POS_LBr+minimummaximumclip_convert_to_torque_from_pwm) r!motor_commands motor_anglemotor_velocitytrue_motor_velocityr%r"r#pd_maxpd_minpwms r&convert_to_torqueMotorModel.convert_to_torqueUs&0 33l;;BBBl;;BBB  4 ; ;>>Bw+ 45R.8PPfBw+ 45R.8PPf RZZ2 2RZZ5J Jc G{3 4r7J Jc ''#tS !C  + +C EEr)c[R"UR[R"U5UR-UR - -[ *[ 5nURb/[R"USUR-UR5n[R"[R"U5UR-URUR-[R"U5-- [*[5nX@R - n[R"U5n[R"U5n[R"XpRUR5n[R"Xh5n[R"UR U5nURb/[R"USUR-UR5nX4$)a]Convert the pwm signal to torque. Args: pwm: The pulse width modulation. true_motor_velocity: The true motor velocity at the current moment. It is used to compute the back EMF voltage and the viscous damping. Returns: actual_torque: The torque that needs to be applied to the motor. observed_torque: The torque observed by the sensor. rG)r+rRrasarrayrrOBSERVED_TORQUE_LIMITrrVOLTAGE_CLIPPINGsignabsoluteinterprrmultiplyr ) r!rZrWobserved_torque voltage_netcurrent current_signcurrent_magnitude actual_torques r&rS&MotorModel._convert_to_torque_from_pwmsgg  C4== (4+;+; ; =  57O &8K8K1K $ 3 35o'' 3$--'  !6!6 6 &' ( (*:):rxsb36 !113    V*V*r)