
    ni+4              	           S r SSKrSSKrSSKrSrSrSrSrSr	Sr
S	r " S
 S\R                  " S/ SQ5      5      rS rS rS rS rS rS rS rS rS r " S S\5      r " S S\5      r " S S\5      rg)z(A Raibert style controller for Minitaur.    N      gy&1?gFx?goʡ?)r      )      c                   :   ^  \ rS rSrSr     SU 4S jjrSrU =r$ )BehaviorParameters    c                 .   > [         [        U ]  XX#XE5      $ N)superr	   __new__)clsstance_durationdesired_forward_speedturning_speedstanding_heightdesired_incline_angle	__class__s         q/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/envs/minitaur_raibert_controller.pyr   BehaviorParameters.__new__   s(     #!4I-FF    )g      ?g?r   gzG?r   )__name__
__module____qualname____firstlineno__	__slots__r   __static_attributes____classcell__)r   s   @r   r	   r	      s'    
 ) #$'"$%F Fr   r	   )r   r   r   r   r   c                     [         R                  " [        5      n[        [        5       HF  nSSUS-  -  -  U SU-  S-      U SU-     -
  -  X'   SU SU-     U SU-  S-      -   -  U[        U-   '   MH     U$ )N      ?r   r   npzeros_NUM_MOTORSrange	_NUM_LEGS)motor_anglesleg_poseis      r   motor_angles_to_leg_poser-   $   s    XXk"(aqAv&,q1uqy*ALQRUVQVDW*WXHK!\!a%%8<APQ	;R%RSHY]  
/r   c                     [         R                  " [        5      n[        [        5       HF  nU [        U-      SUS-  -  X   -  -
  USU-  '   U [        U-      SUS-  -  X   -  -   USU-  S-   '   MH     U$ )Nr#   r   r   r$   )r+   
motor_poser,   s      r   leg_pose_to_motor_anglesr0   ,   s    xx$*a Q/2a.8;2NNJq1u%i!m4a1f~7SSJq1uqy  
r   c                 ~   [         n[        n[        nU S   n[        R                  " U[        R
                  " U5      -  U-  5      nU S   nU[        R
                  " XV-   5      -  U[        R
                  " XF-   5      -  -
  nU[        R                  " XV-   5      -  U[        R                  " XF-   5      -  -
  nXx* 4$ )zThe forward kinematics.r   r   _UPPER_LEG_LEN_LOWER_SHORT_LEG_LEN_LOWER_LONG_LEG_LENmathasinsincos)	r+   l1l2l3extalphaswxys	            r   leg_pose_to_foot_positionrB   4   s    """#
))B#&+
,%{"488EJ"txx'9"99!488EJ"txx'9"99!
R.r   c                 
   [         n[        n[        nU S   nU S   nUS:  d   eUS-  US-  -   nX-  X3-  -   U-
  SU-  U-  -  nX-  X"-  -   SU-  U-  U-  -
  n[        R                  " U5      n	X-  U-   X"-  -
  * SU-  U	-  -  n
[        R
                  " U
5      n[        R                  " U5      nXh-   X2-
  S-  -
  SU	-  U-  -  nUS:  d   e[        R
                  " U5      n[        R                  " XL-  5      U-
  nU* U4$ )zThe inverse kinematics.r   r   r   )r3   r4   r5   r6   sqrtacosr7   )foot_positionr:   r;   r<   r@   rA   hip_toe_sqrcos_betahip_ankle_sqr	hip_anklecos_extr=   hip_toe	cos_thetathetar?   s                   r   foot_position_to_leg_poserO   D   s+   """A!A!
a%.%1q!t+g+-!b&2+>('BG#a"frkH&<<-ii&)g%/0AFY4FG'		'#IIk"'*bg\9a)mg>UV)	Q
))I
%yy%"#sr   c                 :   [         n[        n[        nUn[        R                  " X#-  [        R
                  " U5      -  5      nU[        R
                  " U5      -  U[        R
                  " U5      -  -
  nU[        R                  " U5      -  U[        R                  " U5      -  -
  n[        R                  " Xx-  5      n	[        R                  " US-  US-  -   5      n
[        [        U
S-  U 5      U
* S-  5      n[        R                  " X-  5      nX-
  nU* $ )z]Computes the target leg swing.

Sometimes it is more convenient to plan in the hybrid space.
r   皙?r"   )r3   r4   r5   r6   r7   r8   r9   atanrD   maxmin)foot_horizontal_positionleg_extensionr:   r;   r<   r=   r>   toe_hip_orthtoe_hip_projrN   toe_hip_lenrF   sw_and_thetar?   s                 r   %foot_horizontal_position_to_leg_swingr[   ]   s     """#
))BGdhhsm+
,%dhhuo%TXXc](::,dhhuo%TXXc](::,
))L/
0%
 		,/L!O;<+ c++-EFWZHZ[-=67," *r   c                     [         n[        n[        n[        R                  " X-  [        R
                  " U 5      -  5      nU[        R                  " U5      -  U[        R                  " U 5      -  -
  $ r   r2   )r=   r:   r;   r<   r>   s        r   extension_to_ankle_distr]      sS    """
))BGdhhsm+
,%	dhhuo	TXXc] 2	22r   c                     [         n[        n[        nUS-  U S-  -   US-  -
  * SU-  U -  -  n[        R                  " U5      $ )Nr   )r3   r4   r5   r6   rE   )distr:   r;   r<   rK   s        r   ankle_dist_to_extensionr`      sJ    """!edAgoA%&!b&4-8'	7	r   c                    [         R                  " [        U S-  S5      5      nUS   US   -
  U-  US   -   nUS   US   -   S-  S-
  n[        US5      nSn[	        U5      n[	        US   5      n[	        US   5      n	Xx-
  n
X-
  nXf-  U-
  nXU-  -
  U-  nXf-  U-  U
-
  U-  nX-  U-  X-  -   U-   n[
        n[        n[        [        UUU-
  S-   5      UU-   S-
  5      n[        U5      nUU4$ )	Ng      ?r   r   r   rQ   r"   gffffff?g{Gz?)r6   rD   rT   rS   r]   r3   r4   r`   )phase	init_poseend_posenormalized_phaser?   min_extphi	min_delta
init_delta	end_deltadelta_1delta_2delta_pabdeltar:   r;   r=   s                      r   generate_swing_trajectoryrq      s4   YYs53;23 	il"&661E" q\HQK'1,s2''#%g.)&y|4*%hqk2) "'"'IO'w')!
y7W$/!"22Q5IIJV%""
c%b4("r'D.
9%&#
cr   c                 f    [         R                  " U 5      nUS   US   -
  U-  US   -   nUS   nXE4$ Nr   r   )r6   rD   )rb   rc   rd   re   r?   r=   s         r   generate_stance_trajectoryrt      sC    YYu%il"&661E"#
r   c                   *    \ rS rSrSS\4S jrS rSrg)RaibertSwingLegController   g?g333333?c                 (    Xl         X l        X0l        g r   )_speed_gain_leg_extension_clearance_leg_trajectory_generator)self
speed_gainleg_extension_clearanceleg_trajectory_generators       r   __init__"RaibertSwingLegController.__init__   s     "$;!%="r   c                    UR                  5       nUR                  5       n/ nUR                   H  nUR                  R                  S-  U-  U R
                  X!R                  R                  -
  -  -   nUR                  U R                  -
  n[        UUS9nX4n	U R                  UUR                  U	5      n
UR                  U
5        M     U$ Nr   )rV   )estimate_base_velocity	get_phase	swing_setbehavior_parametersr   ry   r   nominal_leg_extensionrz   r[   r{   swing_start_leg_poseappend)r|   raibiert_controllercurrent_speedrb   leg_pose_setr,   target_foot_horizontal_positiontarget_leg_extensiontarget_leg_swingtarget_leg_posedesired_leg_poses              r   
get_action$RaibertSwingLegController.get_action   s    '>>@M))+EL **

1
1
A
AA
E
U


BBXXXZZ & 2GG";;<>?^Mac *@o
 778K8`8`8GI *+7 +> r   )rz   r{   ry   N)r   r   r   r   rq   r   r   r   r   r   r   rv   rv      s     '*(A>$r   rv   c                   (    \ rS rSrS\4S jrS rSrg)RaibertStanceLegController   皙?c                     Xl         X l        g r   )ry   r{   )r|   r}   r   s      r   r   #RaibertStanceLegController.__init__   s    !%="r   c                    UR                  5       nUR                  5       n/ nUR                   H  nUR                  R                  nUR                  R
                  S-  U-  U R                  X6-
  -  -
  * n[        XqR                  S9UR                  4nU R                  UUR                  U5      n	UR                  U	5        M     U$ r   )r   r   
stance_setr   r   r   ry   r[   r   r{   stance_start_leg_poser   )
r|   r   rb   r   r   r,   r   target_foot_positionr   r   s
             r   r   %RaibertStanceLegController.get_action   s    ))+E'>>@ML ++2FF\\2FFVVYZZ, -/3/?/? - E0G G H ?
.W.WY,BBDo 778K8a8a8GI *+ ,  r   )r{   ry   N)r   r   r   r   rt   r   r   r   r   r   r   r   r      s     #>X >r   r   c                      \ rS rSrSr\" 5       \" 5       \" 5       S4S jr\	S 5       r
\
R                  S 5       r
\	S 5       r\	S 5       r\	S	 5       r\	S
 5       r\	S 5       rS rS rS rS rS rS rS rS rS rS rSrg)!MinitaurRaibertTrottingControlleri	  z-A Raibert style controller for trotting gait.Nc                 &   SU l         Xl        X l        [        SU R                  R                  * 45      nUS   U l        X0l        X@l        XPl        [        U l
        [        U l        U R                  5       U l        U R                  5       U l        g rs   )_time_robot_behavior_parametersrO   r   _nominal_leg_extension_swing_leg_controller_stance_leg_controller_pose_feeback_controllerDIAGONAL_LEG_PAIR_1
_swing_setDIAGONAL_LEG_PAIR_2_stance_setget_swing_leg_pose_swing_start_leg_poseget_stance_leg_pose_stance_start_leg_pose)r|   robotr   swing_leg_controllerstance_leg_controllerpose_feedback_controllernominal_leg_poses          r   r   *MinitaurRaibertTrottingController.__init__  s     DJK 30!d6O6O6_6_5_1`a"21"5D!5"7$<! *DO*D!%!8!8!:D"&":":"<Dr   c                     U R                   $ r   r   r|   s    r   r   5MinitaurRaibertTrottingController.behavior_parameters#  s    $$$r   c                     Xl         g r   r   )r|   r   s     r   r   r   '  s     3r   c                     U R                   $ r   )r   r   s    r   r   7MinitaurRaibertTrottingController.nominal_leg_extension+      &&&r   c                     U R                   $ r   )r   r   s    r   r   +MinitaurRaibertTrottingController.swing_set/  s    ??r   c                     U R                   $ r   )r   r   s    r   r   ,MinitaurRaibertTrottingController.stance_set3  s    r   c                     U R                   $ r   )r   r   s    r   r   6MinitaurRaibertTrottingController.swing_start_leg_pose7  s    %%%r   c                     U R                   $ r   )r   r   s    r   r   7MinitaurRaibertTrottingController.stance_start_leg_pose;  r   r   c                     [        U R                  R                  5       5      n/ nU H!  nUR                  X$   X$[        -      /5        M#     [
        R                  " U5      n[
        R                  " USS9$ )zGet the average leg pose.r   )axis)r-   r   GetMotorAnglesr   r)   r%   arraymean)r|   leg_indicescurrent_leg_poser+   indexs        r   _get_average_leg_pose7MinitaurRaibertTrottingController._get_average_leg_pose?  sj    /0J0J0LM Hoo'.0@AR0STU  xx!H778!$$r   c                 8    U R                  U R                  5      $ )z)Get the current swing legs' average pose.)r   r   r   s    r   r   4MinitaurRaibertTrottingController.get_swing_leg_poseK  s    %%doo66r   c                 8    U R                  U R                  5      $ )z*Get the current stance legs' average pose.)r   r   r   s    r   r   5MinitaurRaibertTrottingController.get_stance_leg_poseO  s    %%d&6&677r   c                     [         R                  " U R                  U R                  R                  5      U R                  R                  -  $ )z'Compute the current stance/swing phase.)r6   fmodr   r   r   r   s    r   r   +MinitaurRaibertTrottingController.get_phaseS  s?    99

!!113595N5N5^5^_ _r   c                     [         R                  " U R                  SU R                  R                  -  5      nXR                  R                  :  a  [
        [        4$ [        [
        4$ )z4Switch the set of swing/stance legs based on timing.r   )r6   r   r   r   r   r   r   )r|   swing_stance_phases     r   update_swing_stance_set9MinitaurRaibertTrottingController.update_swing_stance_setY  sR    4::q43L3L3\3\/\]55EEE!#677!455r   c                     Xl         U R                  5       u  p#US   U R                  S   La7  X l        X0l        U R	                  5       U l        U R                  5       U l        g g )Nr   )r   r   r   r   r   r   r   r   )r|   tnew_swing_setnew_stance_sets       r   update(MinitaurRaibertTrottingController.update`  sd    J$($@$@$B!M Qtq11%o' $(#:#:#<d $($<$<$>d! 2r   c                    U R                  5       nUS   U R                  S   -
  n[        U5      u  p4[        R                  " US-  US-  -   5      nXR-  nU R                  5       nUS:  a  SnU$ X`R                  R                  U-  -  nU$ )Nr   r   r   )r   r   rB   r6   rD   r   r   r   )	r|   stance_leg_posedelta_swr@   rA   rY   horizontal_distrb   speeds	            r   r   8MinitaurRaibertTrottingController.estimate_base_velocitym  s    ..0Oq!D$?$?$BBH$_5DA))AqD1a4K(K!,ONNEAEL #25N5N5^5^5:6; #<ELr   c                 8    U R                   R                  U 5      $ r   )r   r   r   s    r   get_swing_leg_action6MinitaurRaibertTrottingController.get_swing_leg_action{  s    %%0066r   c                 8    U R                   R                  U 5      $ r   )r   r   r   s    r   get_stance_leg_action7MinitaurRaibertTrottingController.get_stance_leg_action~  s    &&11$77r   c                 8   S/[         -  nU R                  5       nSnU R                   H!  nX#   S   X'   X#   S   X[        -   '   US-  nM#     U R	                  5       nSnU R
                   H!  nXS   S   X'   XS   S   X[        -   '   US-  nM#     [        U5      $ rs   )r'   r   r   r)   r   r   r0   )r|   r+   swing_leg_posejr,   r   s         r   r   ,MinitaurRaibertTrottingController.get_action  s    s[ H..0N	A__"%a(hk . 1! 4h9}1fa 
 002O	A#&q)hk / 21 5h9}1fa 
 $H--r   )r   r   r   r   r   r   r   r   r   r   r   )r   r   r   r   __doc__r	   rv   r   r   propertyr   setterr   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   	  s    5 $6#7$=$?%?%A(,=. % % 4 4 ' '     & & ' '
%78_6?78.r   r   )r   collectionsr6   numpyr%   r'   r)   r3   r4   r5   r   r   
namedtupler	   r-   r0   rB   rO   r[   r]   r`   rq   rt   objectrv   r   r   r   r   r   <module>r      s    .   	    F/ 2 F$ 2D3"J. .b :H. H.r   