ó
    Ìnió  ã                   óV   • S r SSKrSSKrSSKJr  SrSrSrSr	Sr
SrS	r " S
 S\5      rg)zMotor model for laikago.é    N)Úrobot_configé   é   é   é   é   é   c                   ó‚   • \ rS rSrSrSSS\R                  R                  4S jrS r	S r
S	 rS
 rS rS r SS jrSrg)ÚLaikagoMotorModelé   aÀ  A simple motor model for Laikago.

When in POSITION mode, the torque is calculated according to the difference
between current and desired joint angle, as well as the joint velocity.
For more information about PD control, please refer to:
https://en.wikipedia.org/wiki/PID_controller.

The model supports a HYBRID mode in which each motor command can be a tuple
(desired_motor_angle, position_gain, desired_motor_velocity, velocity_gain,
torque).

é<   r   Nc                 óP  • Xl         X l        X0l        Ubk  [        U[        R
                  [        R                  45      (       a  [        R                  " U5      U l        O [        R                  " [        U5      U l        X@l        [        R                  " [        S5      U l        g )Nr   )Ú_kpÚ_kdÚ_torque_limitsÚ
isinstanceÚcollectionsÚSequenceÚnpÚndarrayÚasarrayÚfullÚ
NUM_MOTORSÚ_motor_control_modeÚ_strength_ratios)ÚselfÚkpÚkdÚtorque_limitsÚmotor_control_modes        Úe/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/laikago_motor.pyÚ__init__ÚLaikagoMotorModel.__init__"   ss   € ð
 „HØ„HØ'ÔØÑ Ü	M¤K×$8Ñ$8¼"¿*¹*Ð#E×	FÑ	FÜ Ÿjšj¨Ó7ˆÕä Ÿgšg¤j°-Ó@ˆÔØ1ÔÜŸGšG¤J°Ó2€DÕó    c                 ó   • Xl         g)z¢Set 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)r   )r   Úratioss     r!   Úset_strength_ratiosÚ%LaikagoMotorModel.set_strength_ratios2   s
   € ð #Õr$   c                 ó   • Xl         X l        g)zãSet 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)r   r   )r   r   r   s      r!   Úset_motor_gainsÚ!LaikagoMotorModel.set_motor_gains;   s   € ð „HØ…Hr$   c                 ó   • g ©N© )r   Úvoltages     r!   Úset_voltageÚLaikagoMotorModel.set_voltageH   ó   € Ør$   c                 ó   • g©Ng        r.   ©r   s    r!   Úget_voltageÚLaikagoMotorModel.get_voltageK   ó   € Ør$   c                 ó   • g r-   r.   )r   Úviscous_dampings     r!   Úset_viscous_dampingÚ%LaikagoMotorModel.set_viscous_dampingN   r2   r$   c                 ó   • gr4   r.   r5   s    r!   Úget_viscous_damplingÚ&LaikagoMotorModel.get_viscous_damplingQ   r8   r$   c                 óú  • AU(       d  U R                   nU[        R                  R                  L a'  [	        U5      [
        :X  d   eU R                  U-  nXf4$ SnSnSn	Sn
[        R                  " [
        S5      nU[        R                  R                  L aK  [	        U5      [
        :X  d   eU R                  n	U R                  n
Un[        R                  " [
        S5      nO‰U[        R                  R                  L al  [	        U5      [        [
        -  :X  d   eU[        S[        2   n	U[        S[        2   n
U[         S[        2   nU["        S[        2   nU[$        S[        2   nSX’U-
  -  -  U
X8-
  -  -
  U-   nU R                  U-  nU R&                  b\  [	        U R&                  5      [	        U5      :w  a  [)        S5      e[        R*                  " USU R&                  -  U R&                  5      nXf4$ )a@  Convert the commands (position control or torque 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.
Nr   éÿÿÿÿz<Torque limits dimension does not match the number of motors.g      ð¿)r   r   ÚMotorControlModeÚTORQUEÚlenr   r   r   r   ÚPOSITIONr   r   ÚHYBRIDÚMOTOR_COMMAND_DIMENSIONÚPOSITION_GAIN_INDEXÚVELOCITY_GAIN_INDEXÚPOSITION_INDEXÚVELOCITY_INDEXÚTORQUE_INDEXr   Ú
ValueErrorÚclip)r   Úmotor_commandsÚmotor_angleÚmotor_velocityÚtrue_motor_velocityr    Úmotor_torquesÚdesired_motor_anglesÚdesired_motor_velocitiesr   r   Úadditional_torquess               r!   Úconvert_to_torqueÚ#LaikagoMotorModel.convert_to_torqueT   s  € ð0 	ÞØ×3Ñ3Ðð œ\×:Ñ:×AÑAÒAÜÓ ¤JÓ.Ð.Ð.Ø×+Ñ+¨nÑ<€mØÐ)Ð)àÐØ#ÐØ	€BØ	€BÜŸš¤¨QÓ/ÐØœ\×:Ñ:×CÑCÒCÜÓ ¤JÓ.Ð.Ð.Ø8‰8€bØ8‰8€bØ+ÐÜ!#§¢¬°QÓ!7ÑØ	œ|×<Ñ<×CÑCÒ	CäÓ Ô$;¼jÑ$HÓHÐHÐHØÔ-ÐFÔ/FÐFÑG€bØÔ-ÐFÔ/FÐFÑG€bØ+Ü
Ð
1Ô1Ð
1ñ3Ðà!/Ü
Ð
1Ô1Ð
1ñ"3Ðà)¬,Ð*OÔ8OÐ*OÑPÐØ˜"Ð.BÑ BÑCÑDÀrØÑ1ñH3ñ 3Ø5GñH€Mà×)Ñ)¨MÑ9€MØ×ÑÑ&Ü	ˆT× Ñ Ó	!¤S¨Ó%7Ó	7ÜØJóLð 	Lä—g’g˜m¨T°D×4GÑ4GÑ-GØ"×1Ñ1ó3€mð Ð'Ð'r$   )r   r   r   r   r   r-   )Ú__name__Ú
__module__Ú__qualname__Ú__firstlineno__Ú__doc__r   rB   rE   r"   r'   r*   r0   r6   r;   r>   rW   Ú__static_attributes__r.   r$   r!   r   r      sO   † ñð ØØ!Ø".×"?Ñ"?×"HÑ"Hô	3ò #òò	òò	òð ,0÷A(r$   r   )r]   r   Únumpyr   Úpybullet_envs.minitaur.robotsr   r   rG   rJ   rH   rK   rI   rL   Úobjectr   r.   r$   r!   Ú<module>rb      sG   ðÙ ã Û å 6à€
àÐ ð €ØÐ Ø€ØÐ Ø€ôA(˜õ A(r$   