a lɀj{x@sdZddlZddlZddlmZddlZddlZiZ ddZ gdZ ddZ e d Gd d d Z e d Gd ddZGdddZe dGdddZe dGdddZGdddZdS)abJohnny 6's firmware: every baked skill as a COMMAND. The operator's architecture, stated twice and then demanded: "the manual joystick should send the robot commands to act on our automated learnt steps" and "are we not just using that same baked firmware in the free roam?" Until this file, no - free roam was a hand transcription of the laws that kept missing organs (the spin's CoM hold, recover's foot shift, the learned leg speed), and every omission was a fall the exams had already priced away. That is the same disease controller.py was built to kill, one layer up. Structure: - Each skill is a LAW CLASS holding its baked gains and one tick() of pure control logic, registered under a command name. The law tick is EXTRACTED from the stage's exam episode and the episode now calls it - so the exam, the demo and free roam execute the same bytes, and parity is proved by re-scoring every bake (recorded in the commit that introduced this file). - Firmware is the dispatcher: it loads the bakes, owns the shared base layers (bench-interpolated wheel gains, the lean trim, and recover's reflex), maps operator input to commands, and runs the handover glue between laws - the one part that is firmware-only, which is why it lives here where it can be tested headless, not in a web server. Growing the firmware = write one law class with @command, give its exam episode the same tick, and the remote gets the command for free. Laws take plain numbers in and give actuator targets out - no MuJoCo, no files - so this module is the thing that gets transcribed to the real robot's MCU. N)Pathcsfdd}|S)Ncs|_|t<|S)N)nameCOMMANDS)clsr,/home/ghrups/robot-bench/trainer/firmware.pyreg-szcommand..regr)rr rrrcommand,s r ))balance|@) balance_midgy@)balance_crouch@u@cCs|s tdt|}||ddkr0|ddS||ddkrL|ddSt||ddD]V\\}}\}}||kr|kr^nq^||td||}d||||Sq^|ddS)a 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.zno balance bench is bakedrNg& .>?) RuntimeErrorfloatzipmax)Zpts stance_mmhZh0Zg0vZh1Zg1vfrrr interp_gains;s  "rspinc@s&eZdZdZdZddZd ddZdS) SpinLawaThe baked spin skill - on-the-spot rotation at any gate height. Verbatim from spin_episode (SCORE_VERSION 93, baked 5.79): rate ramp (a stepped rate kicks the elliptical topple), yaw-rate servo with its clamp, CoM hold walking the feet under the measured mass, the bank answering roll with direction-aware feedforward, cent trim against the omega-squared pitch shift, and the skater rise. Serves both command shapes: a held rate (the stick / throttle) and a turn-to-heading (indexed quarters, mission verbs).rc Cst|dgddd\ |_}}}} |_|_|_|_|_|_t||_ t dt dt||_ t||_ t dt dt| |_dd|D\|_|_|_|_t||_tt|j|_t|j|_||_||_d|_d|_dS) N ?{Gz?g@皙?cSsg|] }t|qSrr.0vrrr msz$SpinLaw.__init__..)list leg_speedcom_holdbank_kbank_dcent_kbank_ffskaterabsk_yawminrt_maxk_head spin_ratekpkdstationclamprrmathradiansC lean_trim_degtrimleg_ratebrxdtfdxwz_ramp) selfgainsr wheel_gainsbracer@r0r2r3r4rrr__init__es$   zSpinLaw.__init__Nrc Cs$|dur8d} tt||jd|j|_t|j|} nFt| tdkoTt|dk} | r^dn|j} tt |j | | | } |j |jd|j ||j 7_ tt |j||j||j|dd } | td t|d9} td |jd t|jt|}tt |j d d}ttdtd|d| |\}}ttdtd|d| |\}}t|j|d|j|j|d|j}t|j|j||||jtt |j||dd}tt |j | ||j! |j!}tt |d t| d t|}t "||||g|j#||| fS)z=One control tick. Returns (leg_want, leg_rate, u, t, landed).NF@@皙?333333?{Gz?g皙?rr gY@gQgQ?r! rh?@@rп?)$r1r/rBr@r9copysignr:r4rnpclipr3rAr)r*r+r-rr.r; stance_to_legrlean_referencer7r8r?update wheel_commandr5r6r=r,r0r2arrayr>)rCleanlratespeedgzrollrrratecom_offhold_wz remainingZlandedZwz_cmdZwz_capdhZsk_mmZdxchlklhrkrrefutrrrtickwsR $  $z SpinLaw.tick)Nr__name__ __module__ __qualname____doc__STAGErGrkrrrrrWs  rdrivec@s$eZdZdZdZddZddZdS)DriveLawaThe baked cruise skill - hold a commanded road speed. Verbatim from cruise_episode (baked 7.09): the speed loop's own filter before the lean target (raw speed feedback is the drive limit cycle), the acceleration budget on the command, odometry of the SPEED ERROR so asking for speed does not wind the station term against you, and the shove brace fed its own throttle so it cannot brace against the operator.cruisecCsht|ddgdd\|_|_|_|_}}tt||_t dt||_ ||_ ||_ d|_ d|_dS)N皙?333333?rKr)r'r5r6r7r8r; SpeedFilterr/filtraccelr?r@v_cmdoerr)rCrDrFr@v_taurzrrrrGszDriveLaw.__init__c Cs|jtt||j|j |j|j|j7_|j||j}|j||j|j7_t |j ||j|j|j |j ||j|j}t |j|j|||}||jfS)z%One control tick. Returns (u, v_cmd).)r{rrSrTrzr@ryrWr|r;rVr7r8r?rXr5r6)rCwant_vrZr[r\r%rhrirrrrks  z DriveLaw.tickNrlrrrrrss  rsc@s eZdZdZddZddZdS) GovernedDriveaINTERIM drive - bench wheels + cruise's governance, until cruise is relearned on live legs. The baked cruise law was learned on its exam's wheels-only body: the legs there are rigid. On the real robot's live position-servo legs it runs away at EVERY speed (measured: pitch grows to the law's own clamp, wheels saturate, 2.4 m/s faceplant from a 0.4 command). Until the cruise exam runs with live legs (planned with the rock meter, SCORE_VERSION 94) the firmware drives on the balance bench's wheel law at the stand - with trim - governed by cruise's two learned slots: the speed filter and the acceleration budget. This composition measured 1.0 m/s with steering, released clean, before the refactor.cCst|pggddd}dd|D\|_|_|_|_t||_tdt|d|_ tdt|d|_ ||_ ||_ d |_ d |_dS) N)g(@g@rrLrurvrwcSsg|] }t|qSrr"r#rrrr&sz*GovernedDrive.__init__..gQ?rKr)r'r5r6r7r8rr=rr/r}rzr?r@v_fr{)rCZcruise_gZwheel_gr=rFr@grrrrGs zGovernedDrive.__init__c Cs|jtt||j|j |j|j|j7_|j|j|j||j7_t |j |j|jd|j |j ||j|j}t|j|j||||j}||jfS)Nr)r{rrSrTrzr@rr}r;rVr7r8r?rWrXr5r6r=)rCr~rZr[r\rhrirrrrks   zGovernedDrive.tickN)rmrnrorprGrkrrrrrs rstancec@s&eZdZdZdZddZd ddZdS) StanceLawaStand at a height and catch whatever arrives. Verbatim from recover_episode (the standing law every exam rides on): feet slide TOWARD the fall, putting the contact patch back under the mass faster than the wheels can drive it there, and the shove brace leans into a detected push. Wheel gains and trim are supplied by the caller so the same law serves recover's exam (its own baked slots at 460) and the firmware's stance command (bench-interpolated gains at any gate height).recoverc Csbdd|D\|_|_|_|_t||_t||_t||_t ||_ ||_ t||_ ||_ dS)NcSsg|] }t|qSrr"r#rrrr&sz&StanceLaw.__init__..) r5r6r7r8rr=shift_k shift_maxr;r>r?rr@) rCrEr=rrr(rFrr@rrrrGs     zStanceLaw.__init__rc Cst|j||j}t|jd|\}}|j|||j} t |j |||j | } t |j |j||| |j} t||||g|j| fS)z2One control tick. Returns (leg_want, leg_rate, u).rO)r; foot_shiftrrrUrr?rWr@rVr7r8rXr5r6r=rSrYr>) rCrZr[r\odor{dxhpknbrhrirrrrks zStanceLaw.tickN)rrlrrrrrs  rwakec@s4eZdZdZdZdZdZdZdZddZ d d Z d S) WakeLawaStand up from the collapsed heap - the baked rise choreography. Phases: lay out flat, launch pose, then the rise drive BY HEIGHT (gating on roller contact cut the drive at the rock-over and dropped it every time) until the stand pose lands. A wake that has not stood inside its window re-seats and tries again - the rise skill is honestly imperfect (bake 6.36/12) and the real machine would do exactly this.rise)gH@g:@)gJ@gag,@cCst|p gddgddd}dd|D|_tdtdt|jdd |_||_d |_d|_d |_ d |_ t d \}}t ||gd |_t t|jd t|jdgd |_t t|jd t|jdgd |_dS)N)gb=y?g{Pk?g?g(\8@rrcSsg|] }t|qSrr"r#rrrr&-z$WakeLaw.__init__..r!rNrrOrFQ?r)r'rrr1r/hand_mr@phaserjtriesgave_upr;rUrSrYstandr9r:LAYlayLAUNCHlaunch)rCrDr@rZsthZstkrrrrG*s0 zWakeLaw.__init__c Cs|j|j7_|j|jkrp|jd7_d|_d|_|j|jkr\d|_|tddddfS|tddddfS|jr|tddddfS|jdkrt t t ||j tdkrd|_|j td dddfS|jdkr"t t t ||jtd kr d |_|jtd dddfSd}||jkr`t t |jd|jd|d ddd}|j}td tdt dt |jd }|ddkot t t ||jtd k}||||dfS)a]One control tick. Returns (leg_want, leg_rate, drive, done, reseat): drive is the open-loop wheel command during the ascent (None = balance owns the wheels), done means STOOD, reseat asks the caller to put the robot back in the heap for another try (sim: place_sitting; real robot: it is already down there).rrrTgN@NFrIgA@g @rgb@gQ?g(\?r?rurOgr@)rjr@TIMEOUTrrTRIESrr9r:rrSrr/rrrrTrrr1)rChip_zcur_legsrrwantratedonerrrrk;sJ     $z WakeLaw.tickN) rmrnrorprqrrrrrGrkrrrrrs rc@sxeZdZdZddZddZddZdd Zd d Zd d Z ddZ ddZ ddZ ddZ ddZddZddZdS)FirmwareaThe robot's brain between the remote and the laws. Owns the bakes, the base layers, and the mode glue: which law has the legs and wheels right now, and how command changes hand over. The glue rules were each paid for with a measured fall: - the spin law engages only for on-the-spot turns (driving turns are differential - the CoM hold cancels the drive lean and diverges on long legs), and once engaged it LATCHES until the stick releases (the speed estimate wobbles past any entry gate mid-spin, and dropping the legs mid-spin fell a 350 crouch nearly every time); - every law exit hands the legs to a neutral stance hold at that height (freezing "where they are" kept the CoM hold's foot offset baked in - equilibrium off the trim lean, wheels accelerating forever: the mission-end shoot-off); - the stance hold's odometry rebases on entry: hold HERE, never home to where the odometer happened to start. c Cs||_t||_g|_tD]6\}}||}|r|j|t|ddt fq|j |d|_ d|_ d|_ d|_d|_d|_d|_d|_dS)Nrridler)r@rrunsbptsBALANCE_STANCESbakeappendrSrYrsortrecmodelawr manual_legsr~want_wzr)rCr@Zruns_dirnmrrrrrrGs    "  zFirmware.__init__cCs:z tt|j|ddWSty4YdS0dS)Nz _gains.jsonrD)jsonloadopenr Exception)rCstagerrrrs z Firmware.bakecCs8|jr,t|jdkr,t|jd|jdStddS)z1Recover's reflex as a base layer under every law.  rrK)rlenr; ShoveBracerCrrrrFszFirmware.bracecCst|d|j|_d|_dS)Nrr)rrr@rrrrrrcmd_wakeszFirmware.cmd_wakecCsPtt|dd}|jdup,t||jdk}||_|jdkrL|rL||dS)Nt@|@rr)rrSrTrr/r_stand)rCmmchangedrrr cmd_stances zFirmware.cmd_stancecCsL|dur|jdkrd|_dStdd|D|_|jdkrHd|_d|_dS)z|Direct joint sculpting: legs verbatim, wheels balancing. None releases back to a stance hold at the measured height.Nmanual manual_exitcSsg|] }t|qSrr"r#rrrr&rz'Firmware.cmd_manual..r)rrSrYrr)rCZlegs_radrrr cmd_manuals  zFirmware.cmd_manualcCs d|_d|_||jpddS)Nrp|@)r~rrrrrrrcmd_haltszFirmware.cmd_haltcCstt|dd}tt|dd}t|dt|d}}|dkrJd}|dkrVd}|jdkrrt|dkrrd}n"|d|krd}n|d|krd}||_||_dS) aThe semi-automated stick: raw operator axes in, LAW choice out. Forward = the drive law; twist at standstill = the spin law; release = a stance hold where you are. SHAPED for a real 2-axis stick (operator's diagnosis: the demo slider was one pure axis, the joystick is not): a full-left pull always carries vertical leakage, and 12% of wobble was measured flipping the firmware through four modes mid-spin. Deadzone per axis, then axis dominance - the axis you clearly mean wins, the leakage is discarded - and while a spin is latched the vertical axis is dead entirely until the twist releases: mode changes are deliberate acts, not thumb drift.grrrrurrN)rrSrTr/rr~r)rCr%ZwzavZawrrrsticks   zFirmware.stickc Cstt|r|nddd}tt|j|tt ||j rRt |j dkrR|j dnd|j rrt |j dkrr|j dnd|j rt |j dkr|j d nd | ||j |_d |_d|_dS) Nrrrrrwrrurrr)rrSrTrrrr9r:r;r<rrrFr@rrr)rCrrrrrs     zFirmware._standcCsl|d}|durdStt|r$|nddd}t||t|j|||j|_ d|_ d|_ d|_ d|_ dS)NrFrrr rT)rrrSrTrrrrFr@rr spin_settlehold_cmdspin_rel)rCrrrrr_spins  zFirmware._spinc Csrttd|dd}t|j|}tt|d|dt|d}tt|dt|d|||t t |S) zwBench-interpolated balance at the MEASURED height - the wheel law under manual sculpting and the wake's ascent.rOrrrrrrr) rrSrTrrr;rVrXr9r:r<)rCrrZr[r\rZwgrhrrr_bench_us   zFirmware._bench_ucCs^t|dt|jdttd||j |_ t d\}}t ||||g|_d|_dS)Nrtrrrr)rrrrr9r:r;r<rFr@rrUrSrY drive_legsr)rCrrrrr_drives    zFirmware._drivec Cs|d}|d}|d}|j||j7_ddddddd}|jdkr|j|d |d \}}}} } |j|||| d | r||jpd n|jjrd |_d|_|jdkr|dur| |d ||||d<|j|d<|S|jdkr|j|j t d| |d |||d|j|d<|S|jdkrJ|t td|d dd|jdk} t|jdkopt|jdk} t|jdk} |jd krd} } | r|jdkr|d ddkr|jjdkr|d n|nl| r |jdkr t|dkrH||jn>|jdkrH| sHt|jjdkrHt|dkrH||j|jdkrt|jdkrj|jnd}|jt t||jd |jd!|j7_|d"}t|dks|jdurt|jdkrd|_|jj|||||d#|d$|d%t|jd&kr |jnd&d'\}}}}}n|jdur6d|_d|_|jj|||||d#|d$|d%d&d'\}}}}}t|d(kr|j|j7_nd|_t|jdkp|jduot|j|jjd)k}|jd*krd+|d<|r |jd*kst|d,kr ||jp|jj|j||||d-n|jdkr|j|j|||\}}t td.|jd/d(}t t|d0t| d0t|}|j|||jt dd1n6|jdkr|j||||j\}}}|j|||dd|vs|ddus|d|jkr|j|d<|S)2zOne firmware tick. sense: dict with lean, lrate, speed, gz, rollr, rrate, com_off, hip_z, cur_legs. Returns dict with leg_want, leg_rate, u, t, wake_drive, reseat, mode.rZr[r\rNF)rirj wake_drivereseatleg_wantr>rrr)rr>rrrrrirrgn@)rr>rirrOrrrr rMrgz@g`|@rKrrg333333?grHr]r^r_r`gư>)rarQrrJholdg333333?)rr>rirjgffffffֿrPr)rirjrr>)rr@rrrkrWrrrrrr9r:rrSrTr/rr~rrr{rrrrget)rCZsenserZr[r\outrrrrrrZspinningZ want_spinZ want_drivetargetr]rirj_Z _wants_outrrrrks                            $    z Firmware.tickN)rmrnrorprGrrFrrrrrrrrrrkrrrrrks  #   r)rprr9pathlibrnumpyrS controllerr;rr rrrrsrrrrrrrrs&   P$(#Q