a
    j?                    @   s  d Z ddlZddlZddlZddlZddlmZ ddlZddlZ	ddl
ZddlZddlmZ eejjZeed ZdZdZg dZi Zd	d
 ZG dd dZdddZeddedddedddedddedddedddedddedd d!gZd"d# Zdd$d%Zdd*d+Z dd,d-Z!d.d/ Z"e#d0kr8e"  ed1 Z$g dg d2d3g dg d2d3g dg d4d3g d5g d6d3d7Z%g d8Z&d9d: Z'd;d< Z(d=d> Z)ddBdCZ*dDdE Z+ddFdGZ,dHdI Z-dJdK Z.dLdM Z/dNdO Z0ddPdQZ1dRdS Z2ddXdYZ3dd\d]Z4g d^g d_d3e%d`< d dcddZ5g deg dfd3e%dg< ddidjZ6ddkdlZ7g dmg dnd3e%do< i Z8ddpdqZ9ddrdsZ:ddtduZ;g dvg dwd3e%dx< ddzd{Z<dd|d}Z=d~Z>dZ?dddZ@g dZAdgddgddgdgdgddoddgfgdgdgddddgfgd	ZBg dg dd3e%d< dd ZCd	ddZDd
ddZEg dg dd3e%d< dddZFdddZGdZHdd ZIdddZJg dg dd3e%d< dddZKdddZLg dg dd3e%d< dddZMdddZNg dg dd3e%d< dddZOdddZPg dg dd3e%d< eAQeARdd d ddgfddgfgeBd< dddĄZSdddǄZTg dȢg dɢd3e%d< eAQeARdd dʡ ddgfgeBd< ddd̈́ZUdddτZVg dТg dѢd3e%d< eAQeARdd dҡ dddԄZWdddddלZXddل ZYdZZdZ[dddބZ\dddZ]dZ^dd Z_ddgddgd3e%d< eAQeARdҡd d dddZ`dddZag dg dd3e%d< eAQeARdd d dZbdd ZcdddZddddZedS (  a  Learn to balance, on the four numbers the problem actually has.

Everything measured today says the search space, not the algorithm, is what
decided whether this worked:

  a 6594-parameter network fitted the teacher 17x more closely than a linear
  rule and then fell over in 1.95 seconds, while the linear rule balanced 10/10

  random guesses at just two gains balanced 28 percent of the time, and the
  first success came 2.6 seconds into the search

So this hands the policy only what a balancing robot needs to know:

    lean          how far it is tipped
    lean rate     how fast that is changing
    speed         how fast it is rolling
    odometry      how far it has strayed from where it started

and asks for one number back, applied to both wheels. Five parameters with the
bias. They are readable gains, not weights - you can print them, reason about
them, and type them into the robot's firmware.

The score is upright time AND staying put, because "stay upright" on its own is
satisfied perfectly well by rolling away at a constant speed, which is exactly
what this robot did for 112 metres before anyone noticed.

  python learn_balance.py --iters 40
    N)Path)RobotEnvzrobots/wheeled_biped.jsongQ?f   )	kp (lean)kd (lean rate)station
lean clampc                    s@   t |}|sdS t| ddp i  tt fdd| D S )z7The stage's feelings bill: weighted vestibular metrics.        feelNc                 3   s"   | ]\}}|  |d  V  qdS )r	   Nget).0kwtf 1/home/ghrups/robot-bench/trainer/learn_balance.py	<genexpr>      zfeel_charge.<locals>.<genexpr>)FEEL_WEIGHTSr   getattrfloatsumitems)envstagewr   r   r   feel_charge  s
    
r   c                   @   s(   e Zd ZdZdd Zdd Zdd ZdS )	Senseda/  The four numbers, read from the robot's own noisy estimate.

    Speed is the TRUE rolling speed, not the raw wheel encoder: the encoder
    measures wheel-vs-shank, so when the shin rotates during a squat the
    raw reading includes the leg's own motion - the robot thinks it is
    rolling while it is kneeling (operator-diagnosed). All the axes are
    parallel, so the shank's absolute rotation is chassis pitch rate plus
    hip rate plus knee rate, and the compensation is a plain sum - the
    firmware does the same subtraction from its own encoders.c                    sP    | _  fdd}|d|dg| _|d|df|d|dfg| _d	| _d S )
Nc                    s    j jt j tjj|  S Nmodel
jnt_dofadrmujoco
mj_name2idmjtObjmjOBJ_JOINT)nr   r   r   dof  s    
zSensed.__init__.<locals>.dof	l_wheel_j	r_wheel_jl_hipl_kneer_hipr_kneer	   )ejdlegdodo)selfr   r*   r   r)   r   __init__  s    zSensed.__init__c           
      C   s   | j }|j}t|d  |d }t|jd|jj|j	d }d}t
| j| jD ]D\}\}}|t|jj| t|jj|  t|jj|  | 7 }qRd| d }	|  j|	|j 7  _|||	| jfS )Nr      gyro   r	         ?      ?)r1   est_upmathatan2r   sensorsnoisydata
sensordata_rawzipr2   r3   qvelr4   
control_dt)
r5   r1   upleanrater   wjhjkjspeedr   r   r   read  s    	 zSensed.readc                 C   s>   |   \}}}}|\}}}}	t||||	}
t|||||
S )z6The control law, with its clamp, driven by four gains.)rN   Clean_referencewheel_command)r5   grH   rI   rM   r4   kpkdr   clamprefr   r   r   command  s    zSensed.commandN)__name__
__module____qualname____doc__r6   rN   rW   r   r   r   r   r     s   

r         $@r	        |@c
           4      C   s,  t t|ddd}
|
  dd t|
jd D }|D ]@}d|
j| |
j|   |
j|< d|
j| |
j|   |
j|< q4t	
|d \}}t||||g}td	|D ]0\}}t||
jj|
jjt|
jtjj| < q|d d
 |
jjd< |r&t|}dt| ddt| g|
jjjdd< |rXt|d dt|d dg|
jjdd< |r||
jjd< dD ].}|d |
jj|
jjt|
jtjj| < qnt|
j|
j t |
}tj!"|}t#||
j$ }|rt%|j&dt'd|d ddnt% }d}d}d}d}d}| \}}}}tt	(|} d}!d}"|	rbt|
jtjj)d}!t'd|d t*|	 }"t+|D ]^}#|r|#t#d| krt|}dt| ddt| g|
jjjdd< |!durH|#|
j$ }$d}%t|	D ]X\}&}'d|&|"  }(|(|$  kr |(t,dt|'-dd k rn nt|'-dd}%q|%|
jj.|!df< |/ \})}*}+},t	0||+d|}-t	1|||)|*|-| }.t2|)|- |  td k ot2|*dk ot2|+d!k }/|/r|t2|.|
j$ 7 }|t'dt2|.| d" 7 }|.}t3|
j4}0t|
j5D ]T\}1}2|2|v rHt6||7|2 |
j|2  t'd#|
j|2  d$d%|0|1< n|.|0|1< q|
8|0 |t2|+d&t2|*  |
j$ 7 }|#|v r|
jjd  t|9d'dg| 7  < |
j:dk r q||
j$7 }qj|d(krd)n|d*krd+nd,}3t;|
|3 | d-|t'|d.   d/|t'|#d% d%   t<|  S )0a  Balance at ONE held stance. Three benches share this episode -
    straight legs (460), half crouch (400), full crouch (340) - because one
    gain set does not fit every height: the crouch moves the CoM and the
    trim, and per-height searches give bob three proven points to
    interpolate between instead of one compromise. Legs are servo-held at
    the stance (the real robot's position loops, not a weld) and the
    kinematic trim for that stance is in the law, scale fixed at 1.0.bobFseedtask	randomisec                 S   s"   g | ]\}}| d dkr|qS modepositionr   r   iar   r   r   
<listcomp>  s   zepisode.<locals>.<listcomp>	actuatorsr:        @@r-   r.   r/   r0   {Gzd?r7   Q#@r	   gQ#N      r   r+   r,   r;   
      sizechassis皙?      ?皙?      ?dur333333?r(         @皙?333333?&.>r9         ?      i  balanceir  balance_midbalance_crouch      @ư>      D@)=r   SPECreset	enumeratespecctrl_loctrl_hi
act_centeract_spanrO   stance_to_legnparrayrD   r   rA   qposr"   jnt_qposadrr$   r%   r&   r'   r=   radianssincosoptgravityrE   r#   
mj_forwardr   randomdefault_rngintrF   setintegersmaxlean_trim_deg
mjOBJ_BODYlenrangeminr   xfrc_appliedrN   rP   rQ   abszerosact_dimact_idxclipindexstepchoiceup_zr   EFFORT_RATE)4rR   r`   secslean0shove	slope_degslope_step_deg	stance_mmv0pushesr   legsrg   hip0knee0posenmvthsrngr(   hitsZup_seffortZsettle_iZchatprev_urS   rT   r   rU   trimpush_bidZpush_gapr   t_nowr   i2evZt_srH   rI   rM   r4   rV   uquietactjaiZ_feel_stager   r   r   episode  s    	 
,,
*


2
*
	



&
r   ry   )r   {Gz?g333333ӿ)r   r   {Gzr|   333333?)r   r   g@gٿ      @)r   r   g      r:   )r   r   c               	   C   sP   z t ttd d d} W n ttfy:   d} Y n0 | rLt| dgS g S )a  The operator's recorded mouse-pushes, as one extra exam case.

    Loaded fresh per score() call; the server only WRITES the bank while no
    search is running, so within a run the objective holds still. An empty
    bank means no extra case and unchanged scores.runszpush_bank.jsoneventsN)r   )jsonloadopenROOTr   OSError
ValueErrordict)evsr   r   r   
_push_case  s
     
r   c                    s0   t t  }tt fddt|D S )Nc                    s,   g | ]$\}}t  | fd i|qS r   )r   r   rg   crR   r   seed0r   r   r   ri     s   zscore.<locals>.<listcomp>)CASESr   r   r   meanr   )rR   r   r   r   casesr   r   r   score  s    
r   (      ffffff?333333?c	              	      s  t j||du r"t g dn
t |tt}	td|	d t| D ]}
t t 	dfddt|D  t fdd D t fd	d D t 
t  dtd
|d  }t | | g d }|t||  t fdd|D   }t|}||	krH|| }	|rn||
d
 | t|t|	  |r| r q|
d
 d dks|
dkrPtd|
d
 dd|dd|	ddddd D   qP|	fS )a  Augmented random search over five numbers.

    Direct search, not gradients. On a space this small it is both simpler and
    faster: jitter every gain, keep whichever direction helped, step that way.
    Always step from the BEST rule found so far - stepping from the current one
    lets a bad round walk the search downhill, which is how an earlier attempt
    at this produced 30 rounds that never beat their own starting point.
    N)r   rx   r:   Q?z  start 6.2f皙?c                    s   g | ]}  d dd qS )r   r9      )normalr   _)r   scaler   r   ri     r   zsearch.<locals>.<listcomp>c                    s   g | ]}t  |  qS r   r   r   drR   nur   r   r   ri     r   c                    s   g | ]}t  |  qS r   r   r   r   r   r   ri     r   r9   r7   r   c                 3   s&   | ]}| |   |  V  qd S r    r   r   rg   deltasrmrpr   r   r     s   zsearch.<locals>.<genexpr>   r   z  round Z3d  z   best z    c                 s   s   | ]}|d V  qdS )z+7.2fNr   r   r   r   r   r   r     r   )r   r   r   r   r   r   printr   maximumr   argsortr   concatenatestdr   r   tolistjoin)itersdirsr   alphar   r`   g0on_roundshould_stopbestitkeepsdcandr   r   )r   rR   r   r   r   r   r   r   r   search  s4    
"
& 


 "r  c                 C   s$  t d|  t ddddddddd d	i fd
tddfdtddfdtddfdtddfdtddfg}|D ]\}}g g  }}tdD ]X}ttd| ddd}	|	  |dr|d }
t|
d dt	|
d dg|	j
jdd< d|	j
jd< t|	}tjd| }t||	j }|d rLt|jd!|d! dd"nt }t|D ]j}|| }|	t||g ||v r|	j
jd  t|d#d$g|d   7  < |	jd%k rZ qƐqZ||	j |tt|	jd d&  qt d|ddt|d'd(t|d)d* qxd S )+N
r  	conditionz>16r  uprightz>9Zstrayedflatztipped 3 degr   )r   ztipped 7 degQ?ztipped -7 deggQzshoved 1.2 m/sr   r   zshoved 2.0 m/s       @ro   i(#  r   Fr_   r   r7   r   rp   g?r   rr   rt   r   rx   r:     z8.2fzs z8.0fmm) r  r   r   r   r   r   r   r=   r   r   rA   r   r   r   r   r   r   rF   r   r   rW   r   r   rE   r   r   r   appendtr   base_posr   )rR   labelr   rowsnamer   ZupsZstraysrg   r   r   r   r   r(   r   r   r   r   r   r   report  s6     
,*

*
*r&  c                  C   s
  t  } | jdtdd | jdtdd | jdttd d |  }d	d l}| }t	|j
|jd
\}}td| | dd td tt|D ] \}}td|dd|d qt|d t|jjjddd t| tdt|jd td|j  d S )Nz--itersr   )typedefaultz--secsr\   z--outzruns/balance_gains.json)r(  r   )r  r   z
  z.0fzs of learning
z  learned gains:z    Z11sr  z+8.3fz,how it does on conditions it never practisedT)parentsexist_ok)gainsnamesr   z
wrote )argparseArgumentParseradd_argumentr   r   strr   
parse_argstimer  r  r   r  rD   NAMESr&  r   outparentmkdirr   dumpr	  r   )apargsr2  t0rR   r  r(   r   r   r   r   main  s     
r;  __main__r   )g333333@gHzG@gRQ?p=
ף?)r,  r`   )g(\@皙?333333?r=  )
trim scale	leg speedzlevel shiftcom holdrx   rx   r	         )r   r   r   r^   ))r   r]   )r         y@)r        @u@c               	   C   sD   g } t D ]2\}}t|}|r| |t|dd tf qt| S )z<(stance_mm, [kp, kd, station, clamp]) for every baked bench.Nr   )BALANCE_STANCESacceptedr   r   r   r   sorted)ptsr   hrR   r   r   r   balance_pointsY  s     rL  c                 C   s   | st dt|}|| d d kr0| d d S || d d krL| d d S t| | dd D ]V\\}}\}}||  kr|kr^n q^|| td||  }d| | ||    S q^| d d S )aV  Wheel gains at a height, interpolated between the balance bakes.

    Piecewise LINEAR through the baked points, clamped flat at the ends -
    a quadratic through three points can overshoot between them, and an
    overshot clamp or station is a law nobody examined. One point degrades
    to constant gains (the old single-bench behaviour).zyno balance bench is baked - bake balance / balance_mid / balance_crouch first; bob interpolates its wheel gains from themr   r9   r   Nr   rx   )RuntimeErrorr   rD   r   )rJ  r   rK  h0g0vh1g1vr   r   r   r   interp_wheel_gainsc  s    "rR  c                 C   s8   zt tt|  d d W S  ty2   Y dS 0 dS )z7What an earlier stage was signed off with, if anything.z_gains.jsonr+  N)r   r   r   	ROOT_RUNS	Exception)r   r   r   r   rH  z  s    rH         @    T  Fc           0   	      s  | \}}}}	t  }
tt|ddd    t jtjjd} fdddD }d}d	d t	 j
d
 D }|D ]@}d j|  j|    j|< d j|  j|    j|< qpt }ttdd }t|}t| j }d}d}d}t jd }|rtd}t|D ]}| j }|rNt|d d dkrHdnd}nH|rv|dk rbdn|dk rpdnd}n |t|| t| t|  }t jj| d }dt jj|d  d t jj|d  d   }| jd |	||  |  7 }tt |d|d   | dd}t|d |\}}t||||g} t!|| | j}|" \}!}"}#}$t#|
|\}%}&}'}(t$|'|#|$|(})tt%|| }*t&|%|&|!|"|)|*}+t'|!|) |* tdk rt'|"dk rt'|#dk r|t'|+ j 7 }t( j)},t	 j*D ]T\}-}.|.|v rZt ||+|.  j|.  t,d j|.  d d|,|-< n|+|,|-< q -|, |t' j.|d   j 7 }|t't jd |  j 7 } j/dk r qqt't jd | }/t0 d  j1 d|  d|t, j1d!   d"t2|/d  t3|  S )#a}  Hold a series of stance heights without falling over.

    The wheels balance on gains INTERPOLATED between the three balance
    bakes at the commanded height - the benches proved those numbers, bob
    only learns the leg motion around them. Scored on tracking as well as
    staying up, because a robot that ignores the command and stands still
    stays upright perfectly.
    r^   Fr_   rv   c                    s    g | ]}t  jt jj|qS r   r$   r%   r"   r&   r   r   r(   r)   r   r   ri     s   zbob_episode.<locals>.<listcomp>l_wheelr_wheelr	   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   rj   r:   q=
ףp?r7   r   g     @@r  rW  i^  rz   r   h    r9   {Gz?rk   QQ?r}   rw   r~   r   r   r   rx   )4rL  r   r   r   r$   r%   r"   r&   r   r   r   r   r   r   r   r   r   r   rO   r   leg_rater   rF   r   r"  r=   r   r   r   rA   subtree_comxposr   slewrN   rR  rP   r   rQ   r   r   r   r   r   r   r   heightr   r   r!  r   r   )0rR   r`   r   Zheightsyanksettle
trim_scale	leg_speed	lvl_shiftcom_holdbptscom_bidcom_widsfdxr   rg   r   currI   r(   errr   drift_ix0r   r   	target_mmcom_xax_xdxbhip_tknee_twantrH   lraterM   r4   rS   rT   r   rU   rV   r   r   r   r   r   x_endr   r)   r   bob_episode  s    

 


  *



 

r  c                 C   s   | dkrdS | dkr t |d S | dkrFdt | tt |d d S | dkrZt |d S | d	krfd
S | dkrrdS | dkr~dS t |S )a  The ceiling a perfect run could score, for honest display.

    Every stage's score is 'time upright minus its own penalty', except stop,
    which is a fixed 10-point formula - so raw numbers are not comparable
    across stages (5.88 bob vs 2.56 level says nothing until you know one is
    out of 6 and the other out of 5). Bakes record this so the UI can always
    show 'score / ceiling'.
    stopr\   spin      @r^   r}         @rotaterise      (@sitg      &@jumpg      1@)r   r   )r   r   r   r   r   	stage_max  s    	r  c                    s  | dkrt  ddS | dkr,t  ddS | dkr|tt fddtd	D t d	d
dt dtdd
dg S | dkrt S | dkrt S | dkrt	 S | dkrt
 S | dkrt S | dkrt S | dkrt S | dkrt S | dkr"t S | dkr6t S | dkrJt S | dkr^t S | dkrrt S | dkrtt fddtdD S t  S )Nr   rE  r   r   rF  r^   c                    s   g | ]}t  |qS r   )r  r   rR   r   r   r   ri     r   zstage_score.<locals>.<listcomp>ro   T)rj  r   r\   )rk  slopelevelr  cruiseturnbumpslippayloadr  r  r  r  r  recoverc                    s    g | ]\}}t  ||d qS )r  )recover_episode)r   rg   shr  r   r   ri   &  s   )r	   r        @r   )r   r   r   r   r   r  r   slope_scorelevel_score
stop_scorecruise_score
turn_score
bump_score
slip_scorepayload_score
spin_scorerotate_score
jump_score
rise_score	sit_scorer   )r   rR   r   r   r  r   stage_score  sV    




















r  c                  C   s8   t d} | r,t| dkr,t| d | d S tddS )a  The recovery reflex as a BASE LAYER under every stage.

    Learned once, in recover (brace gain and fade, slots 8/9 of its bake),
    then active during cruise, turn, spin, bump, slip and payload - not just
    during recover's own exam. Skills used to inherit only NUMBERS; the
    operator's question "can it recover itself while spinning?" exposed that
    the catching behaviour itself never rode along. Now it does: a stage
    that gets shoved (or shakes itself apart - the post-spin station
    oscillation) braces with whatever recover has learned so far. Unbaked
    recover means no reflex, which is one more reason to keep recover
    strong.
    r  rr      	   r	   r|   )rH  r   rO   
ShoveBrace)br   r   r   base_reflex+  s    r  c                  C   sf   t td d } td}|r0|dd | dd< td}|rbt|dkrb|d |d	  | d< | d
< | S )a  Recover stands on balance's STATION-CAPABLE slots plus bob's legs.

    It used to copy bob's first six wholesale - but bob's objective never
    pays for holding position, so bob's baked station gain is ~0.08, and a
    recover seeded from it drifts away after every catch (the away penalty
    ate the 46% bake). Balance owns slots 0-3, bob contributes only what it
    actually learned: trim scale and leg speed.r  r`   r   Nr   r^   r7   r   r9   r   )listSTAGESrH  r   )rR   Zbalprevr   r   r   _recover_seed>  s    r  c                   C   s&   zt d W n ty    Y n0 dS )zSearch workers yield the CPU to the live view: the demo stutters
    into chunky substeps whenever the pool saturates the cores, which is
    exactly when the operator is watching a candidate learn.   N)osnicer   r   r   r   r   _nice_workerV  s    r  c                 C   s    | \}}}t |tj|td|S )zCOne pool job: score a candidate. Module-level so spawn can find it.dtype)r  r   r   r   r9  r   rR   r   r   r   r   _eval_stage`  s    
r  c                 C   sP   ddl }i |_z(t| t||}|jdd}W d|_nd|_0 t|t|fS )zExam score plus the ROUGHNESS behind it - tick-to-tick jaggedness,
    not amplitude: a sweeping turn is a slow wave and stays cheap, ripple
    and chatter are expensive (operator: smooth the lines, don't flatten
    the move).r   Nroughr	   )r   
FEEL_ACCUMr  r  r   r   )r   rR   r   Z_envmodrawwaver   r   r   measure_wavem  s    r  c                 C   s   | \}}}t |||S r    )r  r  r   r   r   _eval_polish|  s    
r     r  r   Q?c	           #      C   s  t | }	|	du rdS tj|	td}
ddl}ddl}|djtd|	 pJdd t
d}z$|t| |
 |ff\}}|t||  d }|}tjd}t|D ]}|r| r q*||t|
f}g }|D ]6}d	D ],}||
|| | tdt|
   qq|r |dt| g }t|D ](\}}||t| | |ff q,d}g }t||D ]N\}}| \}}|d
7 }|r||t| |||kr|nd|f qh|jdd d |d d |d k r|d d
 }
|d d }|rdd|t|d   } ||d
 || | |
  q|t| |
 |ff\}!}"|
 ||!||"t|!|kof|"|k dW |  |  S |  |  0 dS )zLocal search from the baked gains: minimize wave height, HARD floor
    on the exam score (a flatter law that fails the exam is no law).Nr  r   spawnr7   r   Zinitializerr   )rx   r   r9   g    eAc                 S   s   | d S )Nr   r   )r!  r   r   r   <lambda>  r   zpolish_stage.<locals>.<lambda>)keyr         Y@rx   r   )r+  
raw_before	raw_afterwave_before
wave_afterkept)rH  r   r   r   multiprocessingr  get_contextPoolr   	cpu_countr  applyr  r	  r   r   r   r   Zstandard_normalr   r   r  r   Zapply_asyncrD   r   sortboolcloser
  )#r   roundsr   r  r   tolr  r  on_evalbaserR   _mp_ospoolZraw0Zwave0floorZ	best_waver   r  r   candsZdltsgnZoutsrg   ZcgdoneZscoredor  r  ZpctZraw1Zwave1r   r   r   polish_stage  sv    



 
  r  r   r   c                     s  t  }tj|d tddkr4tjt tdntg D ]|\}}t|}|sVq@t|t	rt
|t|t}|d | d |< q@|D ],\}}|t|k r|tk r|| |< qq@t}|d urt|tkrtj|td|	d urt|	tkrtj|	tdtj|dd l}dd l}|djtd| pLdd td}z"|
rv|
dd| d	  |t ff}td
 d|d t|D ]}tt|fddt|D  fdd D fdd D  }g }|t|D ],}|| |
r|
t|d| d	  qt|d | t||d  tt d td	|d  }t| | g  d }|t||  t! fdd|D   }|t| ff}|
r|
d| d	 d| d	  ||kr.|| }|rT||d	 |t|t|  |r| r qlqW |"  |#  n|"  |#  0 |fS )Nr`   r  r  r   r  r7   r   r  r9   z  [z] start r   c                    s"   g | ]} d dt  qS r   r9   )r   r   r   )rR   r   r   r   r   ri     r   z search_stage.<locals>.<listcomp>c                    s"   g | ]} |    fqS r   r	  r   rR   r   r   r   r   r   ri     r   c                    s"   g | ]} |    fqS r   r  r   r  r   r   ri     r   r   c                 3   s&   | ]}| |   |  V  qd S r    r   r   r   r   r   r     s   zsearch_stage.<locals>.<genexpr>)$r  r   r   r   r  INHERITr   rH  
isinstancer   r   r   r   r   r  r  r  r  r   r  r  r  r  r	  r  r   r  r   imapr   r  r  r  r   	terminater
  ) r   r  r   r  r   r  r`   r  r  r  r  scale_floorcfgsrcr   r  r   siZdiZownr  r  r  r  r  jobsresrr  r  r  Zscr   )	r   rR   r   r   r   r   r   r   r   r   search_stage  s~    	


& 

 

r  )
r   r   r   r   r@  rA  
foot shiftzshift limitz
brace gainz
brace fade)
r  r  rx   r   rx   rx   r|   r~   {Gz?r|   r  r  rW  c           &      C   s  | \
}}}}}	}
}}}}t t|ddd}|  dd t|jd D }|D ]@}d|j| |j|   |j|< d|j| |j|   |j|< qLt	|}t
t|d d	 }t||||ftt||	 |||
t||||j}|j}t||j }t
j|}t|jtd
| td| d	d}d}d}t|D ]}| \}}}}|||||\} }!}"t|| ||j}|t|"|j 7 }t
 |j!}#t|j"D ]N\}$}%|%|v rt
#||$|% |j|%  t%d|j|%  ddn|"|#|$< q|&|# ||v r,|j'j(d  t)|*ddg| 7  < |tt)|j+d |j 7 }|j,dk r< q^q<|j-d|t%|j-d   t.|  S )a  Take a shove and get back to standing still.

    This is the version that measured well (7.50 / 7.15 / 6.27 at 2.0 / 2.5 /
    3.0 m/s). Several attempts to improve it - handing over to the braking law
    from the stop stage, then collapsing both into a single lean-into-the-motion
    term - all scored worse, so it is back to what was measured rather than
    what sounded better.

    Two mechanisms, both continuous, neither switching modes:
      - the feet slide toward the fall, putting the contact patch back under
        the mass faster than the wheels could drive it there
      - a brace leans into a detected push, sized by how hard it was, fading
        because it answers an event rather than holding a posture
    r^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri   <  s   z#recover_episode.<locals>.<listcomp>rj   r:   rk   r7   r   r   rt   r	   r   r   r9   r   r   rx   r   r   )/r   r   r   r   r   r   r   r   r   r   r   r   rO   r   FW	StanceLawr=   r   r   r  rF   re  r   r   r   r   r   r   rN   tickrh  r   r   r   r   r   r   r   r   rA   rE   r   r   r"  r   r!  r   )&rR   r`   r   r   r   rS   rT   r   rU   rl  rm  shift_k	shift_maxZbrace_kZ
brace_fader   r   rg   r   rt  lawrI   r(   r   r   awayr   r   rH   r  rM   r4   r~  r   r   r   r   r   r   r   r   r  )  s^     
$


&
r  )z
brake gainzbrake clampzbrake stancezcrouch speed)r:   ?gɿrx   r  rz   c           1   	   C   sz  | \}}}}	t  }
tt|ddd}|  dd t|jd D }|D ]@}d|j| |j|   |j|< d|j| |j|   |j	|< qFt
|}d}|}ttd	d
|  dd}dtdt|	 }t||j }t||j }d}d}d}d}d}d}t|D ]r}| \}}}}|r:||kr:|jjd  |7  < ||k} | rL|n|}!|tt|!| | |j ||j 7 }t|d \}"}#t|
|\}$}%}&}'tt|}(| st||d|j  }tt|' tt|'|&d||   })n<|du rt|jd }|j}tt| tt||| })t|$|%|||)|(}*|t|*|j 7 }t |j!}+t|j"D ]`\},}-|-|v r|"|#|"|#f|#|- }.t|.|j|-  td|j	|-  dd|+|,< n|*|+|,< qf|$|+ |j%dk r|dur|j| nd}/ddtd|/d     S |dur |du r t|dk r |j| }tt|jd | }0dd|  d|0  t&|    S q dS )zGet up to speed (or take a shove while standing), then stop as fast
    as possible without falling - legs in play: the brake crouch drops the
    CoM while the wheels haul the speed down.r^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z stop_episode.<locals>.<listcomp>rj   r:        z@rE  r  g     t@     |@     r@皙?r	   Nr~   r   rk   rz   r   r   r9         r   rx   r\   r  g       )'rL  r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   rF   r   rN   rA   rE   rO   r   rR  r=   r   r   r   r"  r!  rQ   r   r   r   r   r   r   r   )1rR   r`   r   r  rk  r   Z
brake_gainZbrake_clampZbrake_stZcrouch_speedrp  r   r   rg   r   Z	CRUISE_ST
stance_nowZbrake_mmZrate_mmr(   Zn_runv_cmdZ	x_at_stopZt_stopZt_braker   Zv_peakr   rH   r  rM   r4   ZbrakingZtgt_mmr|  r}  rS   rT   r   rU   r   rV   r   r   r   r   r   alivedistr   r   r   stop_episode|  s     




"
$r   c                    s6   t t fddtdD t ddddg S )Nc                    s    g | ]\}}t  ||d qS ))r  )r   r   r  r   r   ri     s   zstop_score.<locals>.<listcomp>)rx   rz   r  ro   r	   gffffff?)r  r   )r   r   r   r   r   r  r   r  r   r    s    
r  )
r   r   r   r   z
level gainzlevel dampingzlevel limitrA  stance offsetr  )
g rh@gm@gn?gh|?5?      @r   r   gS㥫?gl?gl?r  c                 C   s   t | dt |df}|tvrddl}ttt}| |di di d< ||d d d< td|d  d	|d
  d }tj	dd t
|t|d t|t|< t| S )zA spec with the floor tilted. Baked in, because MuJoCo precomputes the
    frame of static world geoms and setting it at runtime silently does
    nothing - which produced identical results at 3, 6 and 10 degrees.r7   r   Nworldr  bank_deg	pitch_degZ_bank_r   r9   z.jsonT)r*  r   )round_BANKEDcopyr   r   r   r   
setdefaultrS  r6  r7  r0  )r  r  r  r  r   pathr   r   r   banked_spec	  s    r  c           <      K   sH  t | ddg dd \
}}}}	}
}}}}}d}t|d}dd t|jd	 D }|D ]@}d
|j| |j|   |j|< d
|j| |j|   |j|< qVt	|d \}}t
d||||fD ],\}}||jj|jjt|jtjj| < qddd|d  d g|jjdd< g d|jjdd< d|jjdd< t|j|j t|}t||||g}tj|ddd}tt|}d } } } }} d}!t|jd t|jd  }"}#tt||j  D ]}$|$|j  }%|%dk rdn<|%dk r|%d n*|%dk r dn|%dk rdd|%d   nd}&t||& |jj!d< t"|j#d t$d|j#d }'t|j%&d|jj'|j(d }(tt)|
|' ||(  t*| t*|})t"|j#d  t$d|j#d }*tt)||* d d!}+t	t$d"t+d#|d |) |+\},}-t	t$d"t+d#|d |) |+\}.}/t,|t|,|-|.|/g||j }|- \}0}1}2}3t.||2|3|	}4t/|||0|1|4|}5|t*|5|! 7 }| t*|5|j  7 } |5}!t0|j1}6t|j2D ]N\}7}8|8|v rt)||3|8 |j|8  t$d$|j|8  ddn|5|6|7< q|4|6 t*t5t"|j#d t$d|j#d }9t6t|jd |" t|jd |# }:|:dksf|j7d
k rxd%d|j8    S |%dkr|t$d|9d |j  7 }d|%  krdk sn |%dkr|9dkr||j 7 }||:|j  7 }qt$dt|j8|j  };|j8d&|t$|j8d d   d'|t$|j8d   d(|  d||;   t9|   S ))a  The level stage on the tilting table, sensed the way the real robot
    will be: a BNO086 flat on the chassis top reporting absolute tilt vs
    gravity - leg bounce or not. Control and score share that one reference.

    The plate steps and holds (lean, STAY leaned, return); the legs must keep
    the module level to 1 degree, position is secondary, chatter is charged,
    and the joints may move as fast as torque allows - on this reference the
    old accel-pollution feedback loop does not exist.
         @z@r	   Nrr        p|@)r`   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri   (  s   z!level_episode.<locals>.<listcomp>rj   r:   rk   rl   g\(\?g~jth?r   ro   rx   r	   r	   r	   rp   r|   r   )r  unlimited_atr9   rx   r  r  g      @r   r   r   r7   r8   r   r   r   rh?r   r  r  r   r   ):r  platform_envr   r   r   r   r   r   rO   r   rD   rA   r   r"   r   r$   r%   r&   r'   rE   r   r   r   r   re  r=   r   r   r   r"  r   r   rF   ctrlr>   r<   r   r?   r@   rB   rC   r   r   r   rh  rN   rP   rQ   r   r   r   r   r   degreeshypotr   r!  r   )<rR   r`   r   varyr   Z_ignoredrS   rT   r   rU   Zlvl_kZlvl_dZlvl_maxrm  stance_gZdx_kstancer   r   rg   hp0kn0r(   r   r   rt  rI   r   Zroll_errr  Z
settle_penZchatterr   r   rw  y0r   r!  frroll	roll_ratedhZlean_nowdxhlklhrkrrH   r  rM   r4   rV   r   r   r   r   roll_degr  n_stepsr   r   r   level_episode  s    
 
"
. $$ 


.

r&  c                    s>   t ddt ddt ddg}tt fddt|D S )Nr   )r  r  r  c                    s$   g | ]\}}t  |fi |qS r   )r&  r   r  r   r   ri     s   zlevel_score.<locals>.<listcomp>r   r   r   r   r   rR   r   r   r   r  r   r  {  s    r  )r   r   r   r   zslope integralintegral limit)r  r  rx   r   r|   r   r  ra  c           "   
   C   s^  | \}}}}}	}
t t |d }ttd||ddd}|  dd t|jd D }|D ]@}d	|j| |j	|   |j
|< d	|j	| |j|   |j|< q^t|d
 \}}t||||g}t|}t t|}d}d}d}t|jd }tt||j D ]0}| \}}}}||	| |j 7 }tt|t|
 t|
}t||||t|d|j }t||||||| }|t||j 7 }t|j }t|j!D ]N\} }!|!|v rt||"|! |j
|!  t#d|j|!  ddn||| < q|$| |tt|jd | |j 7 }|j%d	k r q<q|j&d|t#|j&d   t'|  S )Nr  r	   r^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z!slope_episode.<locals>.<listcomp>rj   r:   rk   r   r   r   r9   rz   r   )(r=   r  atanr   r  r   r   r   r   r   r   r   rO   r   r   r   r   r   r   r   r"  r   r   rF   rN   r   r   rP   brxupdaterQ   r   r   r   r   r   r   r   r!  r   )"rR   r`   r   	grade_pctr   rS   rT   r   rU   kiki_maxr  r   r   rg   hpknholdr   r   Zslidelean_ir   rw  r   rH   r  rM   r4   rV   r   r   r   r   r   r   r   slope_episode  sT     

 
r4  c                    sN   t ddt ddt ddt ddt ddg}tt fddt|D S )	Nr	   )r-  r\         $      2@g      2c                    s$   g | ]\}}t  |fi |qS r   )r4  r   r  r   r   ri     s   zslope_score.<locals>.<listcomp>r'  r(  r   r  r   r    s    r  z
    <body name="platform" pos="0 0 0.65">
      <joint name="plat_j" type="hinge" axis="1 0 0" damping="200"/>
      <geom name="plat" type="box" size="2.5 2.5 0.08" mass="400"
            friction="1.5 0.005 0.0001" rgba="0.35 0.37 0.42 1"/>
    </body>z
    <position name="plat_a" joint="plat_j" kp="2000000" kv="200000"
              ctrlrange="-0.35 0.35" forcerange="-1e6 1e6"/>c                 C   s   ddl }tt| ddd}|j|j|jd}|dtd }|d	td
 }t	j
||_t	|j|_|  t|jd }|jd| |_|jd| |_|jd| |_|  |S )zA RobotEnv standing on the actuated plate. The platform body is
    appended AFTER the robot so every existing qpos/actuator index is
    untouched; its hinge is the last dof and its actuator the last ctrl.r   Nr^   Fr_   propsz</worldbody>z
  </worldbody>z</actuator>z
  </actuator>rj   )mjcfr   r   spec_to_mjcfr   r8  replacePLATFORM_XMLPLATFORM_ACTr$   MjModelfrom_xml_stringr"   MjDatarA   _rebindr   r   r   	tau_limitr   )r`   r9  r   xmlr(   r   r   r   r    s    r  )r   r   r   r^   r  r  r  r  r  r  r  r  )r   r   )r   r   )r  r   )r   r  )r   rr   r^   )r   r   )r9   r   )	r   r   r  r  r  r  r  r  r  )r   r   r   r   speed filteraccel)r  r  rx   r   r~   r   r  c                 C   s<   | dk rdS | dk r|S | dk r(d| S | dk r8d| S dS )z@Fractional-time speed profile: out, cruise, half, reverse, home.r~   r	   r  r   r:   r?  rD  r   )r   v_topr   r   r   cruise_profile?  s
    
rG  rx   c                 C   s   t t|ddd}|  t|}t| t |j}d }}t||j }	t	|	D ]}
t
|
|	 |}| \}}}}|||||\}}|t||j 7 }|t||g |tt|jd | |j 7 }|jdk rP qqP|jd|  t|  S )Nr   Fr_   r	   r   r:   rz   )r   r   r   r   r  DriveLawr  rF   r   r   rG  rN   r  r   r   r   r   r   	vel_localr   r!  r   )rR   r`   r   rF  r   r   r  ru  effr(   r   r~  rH   r  rM   r   r   r  r   r   r   cruise_episodeE  s      
rK  c                    sF   t ddt ddt ddt ddg}tt fddt|D S )Nr	   )rF  r:   rx   rz   c                    s$   g | ]\}}t  |fi |qS r   )rK  r   r  r   r   ri   `  s   z cruise_score.<locals>.<listcomp>r'  r(  r   r  r   r  \  s    r  )r   r   r   r   rD  rE  yaw gain	yaw clampz
lean-in ffz	roll gainzroll dampingzload balancer  zcurve budget)r  r  rx   r   r~   r   r   r|   gQr	   r  皙r   rx   r  c           G         s`  | \}}}}}	}
}}}}}}}}t t|ddd    dd t jd D }|D ]@}d j|  j|    j|< d j|  j|    j|< qT fddd	D }d
}t	
t|}t }tt|	}tdt|
}
t|}tdtdt|}t fdddD }t }d}d}d}d}d} d }! }" }# }$ }%}&t| j }'t|'D ]}(|(|' })|)dk rxdn|}*|)dk rdn |)dk r|n|)dk r| nd}+|tt|*| |
  j |
 j 7 }| \},}-}.}/||. j}0||0|  j 7 }|+}1t|0dkrNt|+dkrNtdt|t|0 }2tt|+|2 |2}1| jt jd |0|1 |  7 }t jd jj j d }3t	! j"d td j"d }4t jj|d  }5t jj|d  }6|6|5 td|6|5  }7| jd t|+dkrdnd|  7 }||4 ||3  ||7  }8|  jd |8|   7 } tt||| |   dd}9t#tdtd|d  |9 \}:}t#tdtd|d  |9 \};}<t$|t|:||;|<gt	
d! j}t%||0| ||||.| j }=t&|||,|-|=|}>t jd jj j d }?tt| |1|?  | |}@tt|@dt|>  dt|> }@|&t|> j 7 }&t' j(}At j)D ]\}B}C|C|v rt||*|C  j|C  td" j|C  d#d|A|B< n> jd |C d$ }Dtt|>|D+d%r|@n|@  d#d|A|B< q ,|A |!tt j-d |1  j 7 }!|!dt|+|1   j 7 }!|"tt j.d |  j 7 }"t	! j"d td j"d }Et	/|d&  }F|#tdt|E|F d'  j 7 }#|$tt j.d  j 7 }$|)dkr|%tt j-d d(t|E   j 7 }% j0dk r\ q*q\ j1d)|!  d)|"  d*|#  d)|$  d|%  t2|&  S )+Nr^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z turn_episode.<locals>.<listcomp>rj   r:   c                    s(   g | ] } j jt j tjj| qS r   )r"   
sensor_adrr$   r%   r&   mjOBJ_SENSORr   r   r)   r   r   ri     s   
)l_touchr_touchr  r|   r  c              
      s4   g | ],}t  jj jjt jtjj|  qS r   	r   rA   r   r"   r   r$   r%   r&   r'   r[  r)   r   r   ri     s   rl   r	   r   r   ffffff?r   r~   r8   r   r9   r7   g      9@r   rx   ry   rN  r  r  rk   x   r   r   r%  lrn   皙?r  rw   r   )3r   r   r   r   r   r   r   r   r   r=   r   rO   r   r   SpeedFilterr   r   r   r   r   r  r   rF   r   r   r   rN   r,  r?   r@   rA   rB   rC   r>   r<   r   rh  rP   rQ   r   r   r   r   
startswithr   	gyro_truerI  r*  r   r!  r   )GrR   r`   r   rF  wzrS   rT   r   rU   v_taurE  k_yawt_maxZrffZrkZrdr!  r  Za_budr   rg   	touch_adrr  r   r   filtrt  r+  r  oerrff_latgatefb_slowZyerrZverrZrerrZsliperrrk  rJ  r(   r   r   r~  yaw_cmdrH   r  rM   r   r   Zyaw_effZw_capr  Zroll_nowflr  Zimbfbr  r   r"  r#  rV   r   yaw_rater!  r   r   r   r   r  Zroll_refr   r)   r   turn_episode  s     
0

""$


   


$rj  c                    sN   t dddt dddt dddt dddg}tt fddt|D S )Nr	   )rF  r\  r   ry   rw   c                    s$   g | ]\}}t  |fi |qS r   )rj  r   r  r   r   ri   3  s   zturn_score.<locals>.<listcomp>r'  r(  r   r  r   r  )  s    



r  )rw   g333333?g@c                    s    sg S  fddt tD S )Nc              	      sD   g | ]<\}}d | ddd d g|d d gg dg ddqS )	Zridgeboxr  r  r  r	   )r|   g{Gz?gRQ?rx   )rx   {Gzt?g-C6?)r%  r'  ru   posrgbafrictionr   )r   rg   xrK  r   r   ri   B  s   
zbump_props.<locals>.<listcomp>)r   BUMP_RIDGE_Xrq  r   rq  r   
bump_props?  s
    
rs  {Gz?c                 C   sv   ddl }tt| ddd}|rjt|jp&g t| |_|j|j|jd}tj	
||_t|j|_|  |  |S )z3A bob-task env whose world carries the ridge props.r   Nr^   Fr_   r7  )r9  r   r   r  r8  rs  r:  r   r$   r>  r?  r"   r@  rA   rA  r   )r`   rK  r9  r   rC  r   r   r   bump_envI  s    ru  )r   r   r   r   rD  rE  r  rA  )r  r  rx   r   r~   r   r  rx   r  rw   c           *   	      s  | \}}}}}	}
}}t || dd t jd D }|D ]@}d j|  j|    j|< d j|  j|    j|< q:d}t|}t	
t|}t }t fdddD }tt|	}tdt|
}
t }d	}d	}d	 }}t| j }t|d
 \}}t||||g}t|D ]t}| j dk r@d	n|}|tt|| |
  j |
 j 7 }t||| j}| \} }!}"}#||" j}$||$|  j 7 }t||$| ||||"| j }%t||| |!|%|}&|t|& j 7 }t j}'t j D ]N\}(})|)|v rRt||!|)  j|)  td j|)  ddn|&|'|(< q "|' |tt j#d |  j 7 } j$dk r& qq& j%d|  t&|  S )Nc                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri   a  s   z bump_episode.<locals>.<listcomp>rj   r:   r  c              
      s4   g | ],}t  jj jjt jtjj|  qS r   rT  r[  r)   r   r   ri   j  s   rl   r|   r	   rk   rx   r   r   r9   r   rz   )'ru  r   r   r   r   r   r   rO   re  r=   r   r   r   r   r   rY  r   r   r  r   rF   r   r   r   r   rh  rN   r,  rP   rQ   r   r   r   r   r   rI  r   r!  r   )*rR   r`   r   rK  Zv_cruiserS   rT   r   rU   r]  rE  r  rm  r   rg   r  rI   r   r   rt  ra  r+  r  rb  ru  rJ  r(   r0  r1  r~  r   v_wantrH   r  rM   r   r   rV   r   r   r   r   r   r)   r   bump_episode^  sf    
 


 
rw  c                    sF   t ddt ddt ddt ddg}tt fddt|D S )Nr	   rq  rl  rt  gQ?c                    s$   g | ]\}}t  |fi |qS r   )rw  r   r  r   r   ri     s   zbump_score.<locals>.<listcomp>r'  r(  r   r  r   r    s    $r  )r   r   r   r   rD  rE  zgrip ceiling)r  r  rx   r   r~   r   rx   r  c           "      C   s(  | \}}}}}	}
}t t|ddd}|  |jjd d df  }tdtdt|}t	|}t
 }tt|	}tdt|
}
d}d}d }}t||j }t|D ]T}||j }d	| |  kod
| k n  }||r|nd |jjd d df< |dk rdn|}|tt|| |
 |j |
|j 7 }| \}}}}|||j}||| |j 7 }t||| ||||||j } ttt||||| | |}!|t|!|j 7 }|t|!|!g |tt|jd | |j 7 }|jdk r qq||jjd d df< |jd|  t|  S )Nr   Fr_   r   rx   r  r|   r	   ry   g?r:   rz   )r   r   r   r"   geom_frictionr  r   r   r   r   r  rO   rY  r   rF   r   r   r   r   rN   r,  rP   rQ   r   r   rI  r   r!  r   )"rR   r`   r   mu_scalerF  rS   rT   r   rU   r]  rE  Zgripr   mu0Zu_capr   r+  ra  r  rb  ru  rJ  r(   r   r!  on_icerv  rH   r  rM   r   r   rV   r   r   r   r   slip_episode  sJ    
   
r|  c                    sF   t ddt ddt ddt ddg}tt fddt|D S )Nrx   )ry  ry   r   r   c                    s$   g | ]\}}t  |fi |qS r   )r|  r   r  r   r   ri     s   zslip_score.<locals>.<listcomp>r'  r(  r   r  r   r    s    r  )r   r   r   r   r@  rA  zload integralr)  )r  r  rx   r   rx   rx   r|   r   r  p=
ף?  c           /   
      s  | \}}}	}
}}}}t t|ddd    dd t jd D }|D ]@}d j|  j|    j|< d j|  j|    j|< qHt	 } fddd	D }t
 jt
jjd
}d}t|d \}}t||||g}t fdddD }t|}tt|| }t }t
 jt
jjd
}t jj| }t jj| d }t jj| d }d} d}!d }"}#t| j }$t|$D ]}%| s|% j dkr|r||  jj|< || ||  ||   jj| d< || ||  ||   jj| d< d} t||| j}| \}&}'}(})|!||)  j 7 }!tt |!t!| t!|}!t"|	|(|)|
|#|(d j }*t$|||&|'|*||! }+|#t!|+ j 7 }#t% j&},t j'D ]N\}-}.|.|v r t ||(|.  j|.  t)d j|.  ddn|+|,|-< q *|, |"t!t j+d  j 7 }" j,dk r qHq| jj|< | jj| d<  j-d|"t) j-d   t.|#  S )Nr^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z#payload_episode.<locals>.<listcomp>rj   r:   c                    s    g | ]}t  jt jj|qS r   rZ  r[  r)   r   r   ri     s   r\  rv   r	   rk   c              
      s4   g | ],}t  jj jjt jtjj|  qS r   rT  r[  r)   r   r   ri     s   rl   r   r7   r  Tr   r   r9   r   r   )/r   r   r   r   r   r   r   r   r   r   r$   r%   r"   r&   r   rO   r   r   r   re  r=   r   r   r  r   	body_mass	body_iposr   rF   r   rh  rN   r   r   rP   r,  rQ   r   r   r   r   r   r   r"  r   r!  r   )/rR   r`   r   kgat_xZat_zr   rS   rT   r   rU   rl  rm  r.  r/  r   rg   r   
wheel_bidsroot_bidrs  r0  r1  r~  rt  rI   r   r+  bidm0rw  z0loadedr3  r  rJ  r(   r   rH   r  rM   r4   rV   r   r   r   r   r   r)   r   payload_episode  s~     

$$


r  c                    sH   t ddt ddt ddt dddg}tt fddt|D S )	Nr	   )r  rx   r  r   )r  r  c                    s$   g | ]\}}t  |fi |qS r   )r  r   r  r   r   ri   	  s   z!payload_score.<locals>.<listcomp>r'  r(  r   r  r   r  	  s    
r  )rA  rL  rM  zheading gainz	spin raterB  z	bank gainz	bank dampz	cent trimzbank ffzskater rise)g5^I?gjq?gh o?g{	@g*:@gXg<R!?gx&rl  g{Gzrz   r  r9   )r9   r   )r   r9   r       V@     Vr  c           @         s&  t  }tt|ddd    dd t jd D }|D ]@}d j|  j|    j|< d j|  j|    j	|< q:t
 }	 fddd	D }
t jtjjd
}t fdddD }t| |t||t  j}|j}|j} fdd}| }d}d}d}d}d}d } } } }} fdddD }t jd t jd  }}t| j }t|D ]} |  j }!|	 \}"}#}$}%t jd j j! j"d }&||& j 7 }|| }'t#|'t$%dk rt#|&dk r| j7 }nd}|dk r|!dks*|dkrV|dkrV|t&|d k rV|d7 }|t&|k rR|t$%|| 7 }d} j j'| (dd}(t|(d d t|(d d  })}*t$)|)|*pd}+ j j*| },d j j+|
d   j j+|
d    }-t|,d t|-d  |) t|,d t|-d  |*  |+ }.t$, j-d t.d j-d }/t jd j j! j"d }0|j/|"|#|$|&|/|0|.||'d	\}1}2}3}4}5t01||1| j}|t#|3 j 7 }t2 j3}6t j4D ]\}7}8|8|v rt5||6|8  j|8  t.d j	|8  dd|6|7< n> jd |8 d }9tt5|3|97d r|4n|4  dd|6|7< q 8|6 |t.dt#|"| t$%d!  j 7 }t$, j-d t.d j-d }:|t.dt#|:t$%d"  j 7 }|t$)t jd | t jd |  j 7 }t#t j j9|d  };t#t j j9|d  }<|t#|;|<  j 7 } j:dk rj q0qj| | }=t$%t;|dkr`|d t.|dd  ng }>t.d||dkr|dnd }t#|=|> }t$)t jd | t jd | }? j<d#|  d$t=|d#  d|  d%t=|?d$  dt=|t. j<d d  d&t=|t. j<d d'  t>|  S )(Nr^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri   O	  s   z spin_episode.<locals>.<listcomp>rj   r:   c                    s    g | ]}t  jt jj|qS r   rZ  r[  r)   r   r   ri   U	  s   r\  rv   c              
      s4   g | ],}t  jj jjt jtjj|  qS r   rT  r[  r)   r   r   ri   Y	  s   rl   c               	      sV    j } td| d | d  | d | d    dd| d | d  | d | d     S )Nr7   r   ro   r9   )	base_quatr=   r>   )qr)   r   r   true_headingd	  s    &&z"spin_episode.<locals>.true_headingr	   r   r   c                    s(   g | ] } j jt j tjj| qS r   r!   r[  r)   r   r   ri   p	  s   
rq   r9   r8   r7   r   r   rx   ry   ro   r   )hold_wz	remainingr   r%  rW  r}   r  rz   r  rw   r   rU  )?rL  r   r   r   r   r   r   r   r   r   r   r$   r%   r"   r&   r   r   r   r  SpinLawrR  r  rF   re  r   r   r"  r   r   rN   r?   r@   rA   rB   rC   r   r=   r   r   xmatreshaper  rf  rg  r>   r<   r   r  rO   rh  r   r   r   r   r   rZ  r   rE   r   r   r!  r   r   )@rR   r`   r   r   quartersr  rp  r   rg   r   r  r  rt  r  rI   r   r  Zh0_trueh_esttargetZqiZsettled_forZ	completedmissZwobblerJ  r  ZasymZ
wheel_dofsrw  r  r(   r   r   rH   r  rM   r4   gzr  Zxm_fx_fy_fncomZaxpoffZ_rollrZ_rrater~  r   r   r!  landedr   r   r   r   Z_rollwZ_wlZ_wrZturned_trueZ
want_totaldriftr   r)   r   spin_episodeJ	  s     


"("


&"

&(r        0@c                 C   s   t ddt dddt ddddt ddt d	dt d
ddt d
ddt dddt ddg	}g }t|D ]\\}}|t| |t|dfi ddi| |dkrdtt|dk rdtt|  S qdtt|S )Nr   )r  r   )r  r  rw   rY  )r  r  r   )r       f@     v@)r       fg     v)r  r  rX  )r  r   )r  r  )r  g     pr  r   rW  r7   r   )r   r   r   r  r   r   r   r   )rR   r   r   Zscoresrg   r   r   r   r   r  	  s$    



	
r  )zrot ratez
lever slowrB  z
foot placezstatic trimrA  )rw   r:   333333rX  r  r  r  r  c           B   
      s\  | \}}}}}	}
t  }tt|ddd  dd tjd D }|D ]@}dj| j|   j|< dj| j|   j	|< qJt
}t }tfddd	D }fd
ddD }tjtjjd}fdddD }dd t||D \}}}}t|d}d}d}d}d}d}d } } }  }!}"| }#tjd }$t|j }%t|%D ]4}&tdt|dtdt|tt|   }'|tt | |' j |'j 7 }|! \}(})}*}+tj"j#| d },dtj"j$|d  d tj"j$|d  d   }-|jd ||,|-  |  7 }tt || t t|	|+ dd dd}.t%|}/|dk r|/dk rd}n|dkr|/dkrd}tj&d  t'|d t(|   f\}0}1t) fd!d"tt*|0  d# t+|1  d# d D }2|2r$tt|d$ nd}3|d%| |3  }4t,-|4d& |.|\}5}6t|5| |6|5| |6g}7t,.||7t,/|
j}t| d'k r|"tt0t||# 7 }"| }#t,1||*d||2|*dj }8|(|8 |  }9t3t|9t%|9}9tt | |9 ||)  dd}:|!t|:j 7 }!t4j5};tj6D ]N\}<}=|=|v rt ||7|= j|=  td#j	|=  d(dn|:|;|<< qD8|; |tj"j9d$ j 7 }td)krt|tt | dd}|du rt| d*k rj:}|t| j 7 }|ttjd |$ j 7 }| t|,|- j 7 } dtj"j;|d  d+ tj"j;|d  d+   }>|>d,|d&  k rh qqhttjd |$ }?td)kr|nd}@|durd-tdd||   nd}Aj:d|@  |A d|tj:d)   d|tj:d)   d.t|?d  d/| tj:d)   d|"  t<|!  S )0a  Rotate the chassis about the hip axis to a commanded angle, then
    hold it STILL - wheels balancing throughout, legs holding stance with
    the feet walking under the measured CoM. Positive = nose down; the
    circle is open to +/-360 with hard stops (no continuous spinning).r^   Fr_   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri   &
  s   z"rotate_episode.<locals>.<listcomp>rj   r:   c              
      s4   g | ],}t  jj jjt jtjj|  qS r   rT  r[  r)   r   r   ri   -
  s   rl   c                    s    g | ]}t  jt jj|qS r   rZ  r[  r)   r   r   ri   0
  s   r\  rv   c                    s    g | ]}t  jt jj|qS r   )r$   r%   r"   r&   r'   r[  r)   r   r   ri   4
  s   r-   r/   c                 S   s   g | ]}t |qS r   )r   r  r   r   r   ri   6
  r   r	   r   Nr   r   rx   g      ?r9   rb  r   r   gQrd  gʡEgʡE?r  r|   c                 3   s.   | ]&}|d  dko$t |   dkV  qdS )r7   r   r  N)r   )r   nn)_mtgtr   r   r   k
  s   z!rotate_episode.<locals>.<genexpr>r   r   r  rk   r  r   r   g
ףp=
?r7   g?r   rz   r\   )=rL  r   r   r   r   r   r   r   r   r   r   r  r   r   r$   r%   r"   r&   r   rR  r=   r   r  r   r"  r   rF   r   r   r   r   r   r   rN   rA   rf  rg  r   pirI  signanyceilr  rO   r   rh  re  r   rP   r,  r>   r   r   r   r   r   rE   r!  xanchorr   )BrR   r`   r   
target_degr   Zrot_rateZ
lever_slowro  Z
foot_placeZ	static_kirm  rp  r   rg   r   r+  rt  r  r  hip_jidsrS   rT   r   rU   rot_cmdrs  branchthetaZreachZt_arriveZperrrv  Zcom_irJ  ZjitZprev_currw  r(   r   rrrH   lrate_grM   r4   ry  rz  r  _cZ_loZ_hiZ_cross_aheadZ_blZ
stance_effr   r   r~  rV   r1   r   r   r   r   hip_zr  progquickr   )r  r   r  r   rotate_episode
  s     




" 

 
r  c              
      sl   t dddt dddt dddt dddt d	ddt d
ddt dddg}tt fddt|D S )N-   rW  )r  r   irX  Z   irY     iLr`  c                    s$   g | ]\}}t  |fi |qS r   )r  r   r  r   r   ri   
  s   z rotate_score.<locals>.<listcomp>r'  r(  r   r  r   r  
  s    






r  )drivezdrive taperz	lift ratezbalance-on mm)r:   r	   rU  r  r  c                 C   s   | j | j }}d|jdd< |j }|j }d|jdddf< d|jddddf< tdtt| j	|j
j }tt|| j	 D ]}t||| q||jdd< ||jdd< dS )zALL motors off: zero every actuator's gain and bias, let the body
    collapse under gravity to its true rest, then power back on.r	   Nr   r9   ro   )r"   rA   rE   actuator_gainprmr  actuator_biasprmr   r   r  rF   r   timestepr   r$   mj_step)r   r   mr   gain0bias0nstr   r   r   r   limp_settle
  s    

r  ges-8Rg!3|@rl   c              	   C   s   t  D ],\}}|| jj| jjt| jtjj	| < qg d| jjdd< g d| jjdd< d| jj
dd< t| j| j dS )z^Put the robot in the operator's rest: rollers front, wheels behind,
    chassis low and level.)r	   r	   gHzG?r   ro   r  rp   r	   N)SIT_POSEr   rA   r   r"   r   r$   r%   r&   r'   rE   r   )r   r(   r   r   r   r   place_sitting
  s    r  )g      H@g      :@)g     J@g     a      .@c           3   	      s  | \}}}}t  }tt|ddd}|  d|_t| t|d dd t|jd D }	|	D ]@}
d	|j	|
 |j
|
   |j|
< d	|j
|
 |j	|
   |j|
< q^t|}|j|j  fd
dfdddD }ttjjd}fdddD fdddD  fddD t fdddD }tttd ttd gd }tttd ttd gd }td\}}t||gd }td}tdtdtdt| }d}d}d}d}d }t||j  }t!|D ]}||j  }|" \}}} }!d	t# j$|d  d t# j$|d  d   }"d }#d}$|dkr|| }%}& fddd D }#|d!krt#tt|| td"k rd}n|dkr|| }%}& fd#dd D }#t#tt|| td"k rd}t#|j%d }n|| }%}&d$|" td%td&t|k r^t#t&|||"|  d'  d(d)}$n\t#t&d$|" d*d&}'t'||'\}(})}*}+tt(|'},t)|*| d|+}-t*|(|)|||-|,}$t+||%|&|j }|t|$|j  7 }t,|j-}.d}/t|j.D ]n\}0}1|1|	v rBt&||	/|1 |j|1  td+|j|1  d,d|.|0< n"|#d urT|#|/ n|$|.|0< |/d7 }/q|0|. |t# j1| d |j  7 }q|d urtt#|j%d | nd}2t2|d- d.|t|d/  d0  dt|2d1  t3|  S )2Nr^   Fr_   r  r   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z rise_episode.<locals>.<listcomp>rj   r:   c                    s    j t tjj|  S r    r   r$   r%   r&   r'   r   r  r   r   r    s   
zrise_episode.<locals>.<lambda>c                    s   g | ]}t  t jj|qS r   )r$   r%   r&   r'   rQ  r  r   r   ri     s   r  rv   c                    s   g | ]} |qS r   r   r[  jqr   r   ri     r   rq   c                    s$   g | ]} j t tjj| qS r   r#   r$   r%   r&   r'   r[  r  r   r   ri     s   
c                    s   g | ]}t  j| qS r   r   r   r   rh   r   r   r   ri     r   c                    s   g | ]}t  j| qS r   r  rQ  )r   r  r   r   ri     s   rl   r   r9   r7   Q?     A@     b@rz   r~   gQ?r	   c                    sN   g | ]F}t td t  j|  |   dt  j|    ddqS       r|   r   r9   r   r   r   r   rE   r   r   lock_qwd_adrwq_adrr   r   ri   0  s   r  r|   r  c                    sN   g | ]F}t td t  j|  |   dt  j|    ddqS r  r  r   r  r   r   ri   8  s   rk   g      i@r  g(\?r   rx   rF  r   r   r  r  r   r_  r   )4rL  r   r   r   min_hr  r  r   r   r   r   r   r   r   r"   rA   r$   r%   r&   r   r   r   r=   r   RISE_LAYRISE_LAUNCHrO   r   r   r   r   r   rF   r   rN   r   r  r"  r   rR  r   rP   rQ   rh  r   r   r   r   r   rg  r   r   )3rR   r`   r   r  ZtaperZ	lift_rateZbal_mmrp  r   r   rg   r   r  Zchassis_bidrt  laylaunchst_hst_kstandZslowZliftrZLAUNCH_HIP_Zphaseh_sumrJ  Zx_ascentr(   r   r   rH   r  rM   r4   r  uwr   r  rate_nowZeff_strS   rT   r   rU   r   rV   r   wheel_ir   r   Ztravelr   )r   r  r  r  r  r  r   rise_episode
  s    
 





 

$



 
r  c                    s2   t  t  t  g}tt fddtdD S )Nc                    s   g | ]}t  |qS r   )r  r   r  r   r   ri   n  r   zrise_score.<locals>.<listcomp>ro   )r   r   r   r   r   r(  r   r  r   r  k  s    r  ))r  rx   )r  r   )     w@r  )rF  rU  )r  r\   )g     @p@g      ,@)g     k@r6  )r  g      6@c                 C   s   t }t| }||d d kr(|d d S ||d d krD|d d S t||dd D ]L\\}}\}}||  krz|krVn qV|| ||  }||||     S qV|d d S )zThe MIRROR fold's trim, measured by drift-null (14 Aug): almost
    exactly the negated normal curve - the off pose lives on the other
    side of the straight-leg singularity and leans the other way.r   r9   r   N)MIRROR_TRIMr   rD   )r   rJ  r   rP  t1rN  r:  r   r   r   r   mirror_trim_degw  s    "r  z	down ratezsettle rate?rU  r  c           C   
      s  | \}}t dpg d}t|g d d d \}}}}	t }
tt|ddd}|  d|_|j|j fddt	j
d	d
dd\}}d|fd|fd|fd|ffD ]\}}|j|< qg djdd< g djdd< d
jd d < t dd t|jd D }|D ]B}d|j| |j|   |j|< d|j| |j|   |j|< qt|}ttjjd}fdddD  ttjjd} fddtjD }fd dd!D fd"dd!D t||||g}ttd td gd# }t	|}t !d$t"dt#d%t$| }d&}d't"dt#d%t$| }d
}d}d d}d  }} d
}!d
}"d
}#d }$t%||j& }%t|%D ]r}&|' \}'}(})}*d}+tj(D ]P},j)|, }-||-j*kr|-j+}.n||-j+kr|-j*}.nq|.|vrd(}+ qqd
}/d }0|dkrVt#d)|||j&  }t,j-| d }1dt,j. d  d t,j. d*  d   }2||j&d+ |	|1|2  |  7 }t,t/|d,|d-   | d.d/}3t	j
|d- |3dd\}4}5t	0|t|4|5|4|5g||j&}t1|
|\}6}7}8}9t	2|8|)|*|9}:t !t3|| };t	4|6|7|'|(|:|;}/|d0ks>|+r|d1k rd*}fd2dD nt	0||||j&}fd3dd4D }0|st,t#t$|| t !d5k rd(}|j5}$j67 }j87 } d
j6d d df< d
j8d d d*df< |#t$|/|j& 7 }#t9|j:}<d}=t|j;D ]n\}>}?|?|v rft/||<|? |j|?  t#d6|j|?  d7d*|<|>< n"|0d urx|0|= n|/|<|>< |=d*7 }=q|=|< |!t,j.| d# |j& 7 }!t#|"t$|(}"t >t ?|j@d* t#d8|j@d# }@t$|@d9kr qq|d ur"|j6d d < | j8d d < |oHt$t,jd d:k oH|jAd;k}A|$d urjd<t#d
d|$|   nd
}BtB|d= d>d|!t#|j5d8 d,    d?|Ardnd
  |B dt#d
|"d@   tC|#  S )ANr^   rC  r   Fr_   r  c                    s    j t tjj|  S r    r  r  r  r   r   r    s   
zsit_episode.<locals>.<lambda>r  r	   rx   )r  r-   r.   r/   r0   )r	   r	   gHzG?r   ro   r  rp   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   zsit_episode.<locals>.<listcomp>rj   r:   rv   c                    s   g | ]}t  t jj|qS r   r$   r%   r&   r   r[  r  r   r   ri     s   r\  r  c                    s"   h | ]}t j|  v r|qS r   )r   geom_bodyid)r   gi)rr  r  r   r   	<setcomp>  s   zsit_episode.<locals>.<setcomp>c                    s   g | ]} |qS r   r   r[  r  r   r   ri     r   rq   c                    s$   g | ]} j t tjj| qS r   r  r[  r  r   r   ri     s   
r7   r  r   r  r  Tg     @e@r9   rb  r_  rk   rc  rd  g     `e@rF  c                    s   g | ]}t  j| qS r   r  r  r  r   r   ri     r   c                    sN   g | ]F}t td t  j|  |   dt  j|    ddqS r  r  r   r  r   r   ri     s   r  r\   r   r   r   g      I@r   r?  r  r  r   r   r  )DrH  r  rL  r   r   r   r  r"   rA   rO   r   r   rE   r$   r   r   r   r   r   r   r   r   r%   r&   r   
mjOBJ_GEOMr   ngeomr   r   r  re  r=   r   r   r   r   r   rF   rN   nconcontactgeom1geom2r   rf  rg  r   rh  rR  rP   r  rQ   r!  r  r  r  r   r   r   r   r   r  r>   r<   r   r   r   )CrR   r`   r   Z	down_rateZsettle_ratebobgrl  rm  rn  ro  rp  r   r  r  r   r   r   rg   r   rq  floor_gtyre_grt  heaprI   Zsettle_rrx  Zdmmrs  r  r  r  r  r  Z	peak_raterJ  Zt_doner(   r   rH   r  rM   r4   groundedcir   otherr   r  ry  rz  r{  r|  r}  rS   rT   r   rU   rV   r   r   r  r   r   r  Zstillr  r   )rr  r   r  r  r  r  r  r   sit_episode  s$   "














(
r  c                    s"   t t fddtdD S )Nc                    s   g | ]}t  |qS r   )r  r   r  r   r   ri   '  r   zsit_score.<locals>.<listcomp>ro   )r   r   r   r   r  r   r  r   r  &  s    r  ) Zcrouchz	dive ratezlaunch driveZtuckZabsorbzrecover ratezair gainzhip trimzlaunch dampz	fire leanz	hip ratiozcom setzhip holdz
knee boostz	tuck ratez
chord tiltztuck atztuck hipzknee cancelz	fire ratez
launch topz	fire gatezbrace atz
fire steerztuck balancez
land catchzair spinzroll fixz	catch hipz
catch flexz
catch kneez	land lean) ghsgvq@g1Zg1Zdۿg(\?g|a2U0?g-1?gY8mܿgj+?giqgףp=
׳?g~:?g\ Ac̝?gHzGgtgnڿg٬\mͿg$۷gz6>Wg{/L
g&WڿgZӼ?gqh ǿgp_Q?r	   r	   r	   r	   r	   r	   r	   r	   r  ))     @k@gfffffP@gfffff`)g33333m@g     L@g_)g!p@gffffffG@gfffff])gaq@g33333F@gY\)gr@g333333E@g33333Z)g33333s@gffffffC@gX)g33333#u@r  g     V)g33333cv@g>@g     T)g33333w@g333333:@gLR)gx@g      5@gffffffO)g$z@gffffff-@g     @I)gd{@g@gA)     |@g333333r5  c                 C   s   t }| |d d kr0|d d |d d  }}n| |d d kr\|d d |d d  }}nzt||dd D ]f\\}}}\}}}	||   kr|krnn qn| | td||  }
||
||   }||
|	|   } qqnt|t|fS )z;Hip/knee radians on the measured tether path, clamped ends.r   r9   r7   r   Nr   )	JUMP_TRAJrD   r   r=   r   )Zh_mmrJ  hhkkrN  Zp0Zk0rP  Zp1k1r   r   r   r   	jump_traj  s    &r
  c                     s  t | dgd  d d \ }}}}}}	}
}}}}}}}}}}}}}}}}}}}}}} }!}"}#ddtt|dd  }$ttdd|  dd	}%d
dtt|dd  }&ddtt|dd  }'ttd| dd
}(dtdtdt| })ttd| dd}*ttdd|  dd}+ttdd|  dd},ttdd|  dd}-dtdtdt| }.dtdtdt| }/dtdtdt|	 }0t td pdgdg d d! }1|1d" pd}2t }3t	t
|d d#d$}4|4  d%|4_|4j|4j fd&d'tjd(| }5t|5d)d*}6tt|5d+d}7t|5d,d-}8t|6d\}9}:d.|9fd/|:fd0|9fd1|:ffD ]\};}<|<j|;< q|dd|6d2 gjd"d3< t|7d dt|7d dgjd3d4< djd d < |8jd"< t d5d6 t|4jd7 D }=|=D ]B}>d8|4j|> |4j|>   |4j |>< d8|4j|> |4j|>   |4j!|>< qt"|4}?t#tj$j%d9}@fd:d6d;D }Afd<d6d=D fd>d6d=D j&t#tj$j'd/ }BD ]}C|8d? j|C< qtj(d d d!f }Dt)|9|:|9|:g}Ed|6 }Fd }Gd"}Hd}Id}Jd}Kd"}Ld d }Md}Nd}Od}Pd}Qd}Rd}Sd}Td }Ud }Vd}Wt#tj$j*d@}Xd}Yd}Zd }[t+||4j, }\|d ur|-dAnd }]t.|\D ]}^|^|4j, }_|?/ \}`}a}b}c|]d ur*|]0t1|_d3|HtjdB tjdC ttj2|Ad"  dC tj2|Ad!  dC d? f d#}dt.j3D ],}ej4|e }f|@|fj5|fj6fv r8dD}d qfq8d}gd }h|HdEv r(|Hd"kr|_dkrt|+|F|.|4j,  }F|F|+d8 krXd!}Hd}In|I|4j,7 }ItdF| }i|IdkrXttjdC |&k rXt|bdk s
|IdGkrXt|`|i tdHk s,|IdkrXdC}Hd}I|+}Gt|4j7d" }MfdId6D t|Fd |Hd!krp|*nd\}j}kt8|Et)|j|kgdC td|4j,}Et9|3|F\}l}m}n}ot:|n|bd|o}ptt;|F|2 }q|Hd!krtdd|Id
  }r|q|r tdF| d!|r   }qt<|l|m|`|a|p|q}gn|HdCk	rRt|%|G|$|4j,  }G|(|Gd  }st|Gd |sd! }kt|+d |(|+ d d" }ttdJ|(dJ d" }ut|+d |(|+ d d! }vtdJ|(dJ d! }wtjd/ }xtt|x|v |w|v  dd}ytt|ddK}ztd|y|z tdd|z  }{|ttd|dL |{ |u|t   }j|ktt||k|x  dMdN }k|jtdO| tt||a dPdQ 7 }jt)|j|kgdC }Ettd| ||a  dRd!gdC }h|I|4j,7 }Ittj2|Ad"  dC tj2|Ad!  dC }|t|W||d? }W|ds|Id%krd3}Hd}Jd!}Ltjd" }NtjdC }OtjdB }P|_}Vn|Hd3kr|d	sl|J|4j, nd}Jt|K|J}Kt|QttjdB }Qtjd/ }}tdJdd! }~|Ld"k
r0tdJd\}j}kt8|Et)|j|kgdC tdS|4j,}E|~tdTdT|   }|}|k
s(tjdC dk rd!}Lnh|Ld!kr t|,d d\}j}k|j|7 }j|jtt|tj|B  ddU7 }j|ktt||a dVdW7 }kt8|Et)|j|kgdC tdX|4j,}EtjdC }ttj2|Ad"  dC tj2|Ad!  dC d? }|dYk r|td|  |'k rdC}Lnxt|-d d\}j}k|jtt|tj|B  ddU7 }j|ktt||a dVdW7 }kt8|Et)|j|kgdC t|)|4j,}Ett|
|`tdF|#  dO |a  dZdtt|tj|B  dZdtddtdt|   fd[d6dED }httj2|Ad"  dC tj2|Ad!  dC }|t|W||d? }W|dr|KdkrdB}H|-}F|_}[t|4j7d" }Unrtd\|F|0|4j,  }Ft|Fd d\}j}k|[d urtdd|_|[ d  }|jttd8|  |!|`  d]d8| 7 }j|ktt|"|` d]d8| 7 }kt8|Et)|j|kgdC td|4j,}Et9|3|F\}l}m}n}ot:|n|bd|o}ptt;|F|2 }qt<|l|m|`|a|p|q}g|[d urtdd|_|[ d)  }|gtt|dO|` |a  dZd| 7 }g|gtt||b dZdtdd|_|[ d   7 }g|Zt|g|4j, 7 }Zt=|4j>}d"}t|4j?D ]n\}}||=v rlt|E|=@| |4j |  td^|4j!|  dRd!||< n"|hd ur~|h| n|g||< |d!7 }q"|4A| tt)jBd dB d_|Dd dB  k}|C r|Y|4j,7 }YtDtdtdZ|4jE}|HdCkr|[d u rt|R|}RnL|[d ur^t|S|}S|_|[ dk r^t|TtFtdtdtjG|X d` }T|HdCkrz|4jEdQk rz q|[d ur|_|[ dakrt|bd
k r|Fdbkr qqdt|4j7dC  }|[d uo|4jEdckottjd" d)k o|ddko|Stdek o|Ttdfk }|Md ur8t|4j7d" |M nd}|Ud ur\tt|4j7d" |U nd}|O|O }|N|N d-|Q dC  }||| dg  }|d ur|jHd|W |N|O|P|Q|R|S|T||V||Y|Z||K||[tI|4dhdi |sdZS tI|4dh djt|Wdk dl  d dtddt|d8    dmtdt|d8   dtd|d   dHtd|TtdO   dmtd|Vp`dd   d|  dNtd|Qd8   dOtd|Rd   dntd|Yd-   tJ|Z  S )oNr	       g     @@rx   gffffffr  r   r  g     |@r   gr  r~   gr   g333333ÿrk   r|   r   rc  r   r  g      T@r  rF  g     @o@r  g     l@rE  r   g     @r^   r9   r   Fr_   r  c                    s    j t tjj|  S r    r  r  r  r   r   r    s   
zjump_episode.<locals>.<lambda>r  ry   r_  r  g      пr   r-   r.   r/   r0   rm   ro   rp   c                 S   s"   g | ]\}}| d dkr|qS rc   r   rf   r   r   r   ri     s   z jump_episode.<locals>.<listcomp>rj   r:   r  c                    s   g | ]}t  t jj|qS r   r  r[  r  r   r   ri     s   r\  c                    s   g | ]} |qS r   r   r[  r  r   r   ri     r   rq   c                    s$   g | ]} j t tjj| qS r   r  r[  r  r   r   ri     s   
r;   rv   tracer   r7   Tr  r   r  r  c                    s   g | ]}t  j| qS r   r  r  r  r   r   ri   x  r   r  gffffff?r  r  r   rU  gffffffֿr   r   g      @r  rU  g333333r   g     p@rN  r   c                    sZ   g | ]R}t t d t j|  |   dt j|       ddqS )g      r   r   r9   r  r   )_wlock_wspinattr   r  r  r  r   r   ri     s   r  rD  r   g
ףp=
?r   r\   g     `|@r  r  g     F@g      4@r   r  )Zpeak_mmvx_lovz_lowy_lowy_pktilt_pk
catch_tilt	back_tilt	land_rollt_lift	leak_fracsat_trJ  r  free_tr  landed_tr
   g      @r  r>  rz   r}   )Kr  r   r   r   r   r   r   rH  rL  r   r   r   r  r"   rA   r   r   uniformr=   r   rO   r   r   r   r   rE   r$   r   r   r   r   r   r   r   r   r%   r&   r  r#   r'   actuator_forceranger   r   r   rF   r   r   rN   r   r  rg  r  r  r  r  r"  rh  rR  rP   r   rQ   r   r   r   r   r   actuator_forcer  acosr   asinr  r,  r   r   )rR   r`   r   r4  Zg_crouchZg_diveZg_launchZg_tuckZg_absorbZg_recZg_airZg_tiltZg_dampZg_fleanZg_ratioZg_dxsZg_holdZg_kboostZg_trateZg_chordZg_tuckatZg_thipZ	g_kcancelZg_frateZg_ltopZg_fgateZg_braceZg_fsteerZg_tbalZg_catchZg_aspinZ	g_rollfixZg_chipZg_cflexZg_ckneeZg_lleanZext_rateZtop_mmZvz_gateZ	brace_ttiZchord_kZtuck_rZdxsZ	crouch_mmZtuck_mmZ	absorb_mmZdive_rZlaunch_rZrec_rr  rl  rp  r   ZerngZstance0Zpitch0Zvx0r  r  r   r   r   rg   r   r  ZwidsZkn_dofrh   Zfrangert  r  Zhcmdr  ph_tZair_tr  Zair_modeZx_launchr  r  r  r  r  r  r  Zx_landr  peakZch_bidr  rJ  r  r(   r  r   r!  rH   r  rM   r4   r  r  r   r   r  Z	fire_leanr|  r}  rS   rT   r   rU   rV   r   Z_fZ_dxlZ_chZ_ehZ_ckZ_ekZ_ka_progZ_hldZ_relr\  Z_ka3Z_ek3Z_t_atZ_vz3Z_hw3Z_cfadZ_fader   wir   r   ZfsatZ	_tilt_nowr  r  r  r  Ze_upZe_leakr  r   )	r  r  r  r   r  r  r  r  r  r   jump_episode  s   "
"










"














 







*










r&  c                    s8    fddt dD }tdt| dt|  S )Nc                    s   g | ]}t  |qS r   )r&  r   r  r   r   ri     r   zjump_score.<locals>.<listcomp>r  r:   )r   r   r   r   r   )rR   r   r   r   r  r   r    s    	r  )r\   r	   r	   r	   r	   r]   r	   N)r\   r   r]   )	r   r   r   r   r\   r   NNN)r\   )rU  rV  FF)rU  )rU  )r  rU  r  r   r  NNN)r   r   rU  r   r|   r   r   NNNNr   )rU  r  rW  )rU  rz   r  r	   )rU  )r	   )rU  rU  N)rU  )r\   r\   ra  )r\   )r   )rU  rx   )rU  )rU  r:   rx   )rU  )r   rt  )rU  rt  rw   )rU  )rU  rx   rw   )rU  )rU  r:   r	   r}  r~  )rU  )r  rW  r  N)r  )r  r  r]   )r  )r   )r  )r  )r  )r  )r   N)r  )fr[   r-  r   r=   r  pathlibr   r$   numpyr   
controllerrO   firmwarer  r   r   __file__r5  r   r0  r   r   SCORE_VERSIONr3  r   r   r   r   r   r   r   r   r  r&  r;  rX   rS  r  rG  rL  rR  rH  r  r  r  r  r  r  r  r  r  r  r  r  r   r  r  r  r&  r  r4  r  r<  r=  r  STAGE_ORDERr  rG  rK  r  rj  r  rr  rs  ru  rw  r  r|  r  r  r  insertr   r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r  r
  r&  r  r   r   r   r   <module>   s    u	9  
 




	
  
&

	8
  
`
-

  
:   
g
J
P%
#a#
,%

"
# 


0
%
 
?!
 
 9
 !
m
   
 	   ^