a lɀjT@s|dZddlZddlZddlZddlmZmZmZddl m Z da GdddZ GdddZ Gd d d ZGd d d ZdS) aYRobotEnv: turns a spec into a learnable task. Two rules kept strictly: 1. The policy only ever sees what the spec's sensors publish, at their real rate, with noise, bias and latency. No cheating on privileged state. 2. The reward may use ground truth, because the reward only exists at training time and never ships to the robot. N) load_spec spec_to_mjcf find_joint) make_taskc@s8eZdZdZddZddZddZdd Zd d Zd S) SensorBankzBZero-order-hold + noise + per-episode bias over MuJoCo sensordata.c Cs||_||_g|_|dD]}|dds,qt|tjj|d}|dkr^td|dd|j |}|j |}dt |d d } |j |dt |||t|| t |d d t |d d t |ddt|t|d d qtdd|jD|_dS)Nsensorsin_obsFnamerzsensor z missing from compiled model?Zrate_hzeAnoisebiasscale) r slicedimperiodr bias_magrrheldnext_tcss|]}|dVqdS)rN).0err'/home/ghrups/robot-bench/trainer/env.py 3z&SensorBank.__init__..)rng control_dtentriesgetmujoco mj_name2idmjtObj mjOBJ_SENSORKeyError sensor_adr sensor_dimfloatappendrintnpzerossumr) selfmodelspecrrssidadrrrrrr__init__s2      zSensorBank.__init__cCsN|jD]B}|jj|d |d|dd|d<t|d|d<d|d<qdS)Nrrsizerrr r)rruniformr*r+)r-rrrrreset5s $zSensorBank.resetcCst|j}d}|jD]}||dkrtj||dtd}|dr^|jjd|d|ddnd}||d ||d <||d |d<|d |d ||||d<||d7}q|S) Nrrrdtyper r rr4rrrr)r*emptyrrasarrayr'rnormal)r- sensordatatoutirrawr rrrread;s   & zSensorBank.readcCsZ|jD](}|d|kr|d|dp&dSq||}|rPtj||tdStdS)zThe corrupted reading for a named sensor, matching what the policy sees, so the estimator never gets a cleaner signal than the network.r rrr r8)rrr*r;r'r+)r-r r=rArslrrrnoisyHs    zSensorBank.noisycsRg}|jD]Bddkr*|dq |fdddddD7}q |S)Nrr csg|]}dd|qS)r .r)rcrrr Wrz%SensorBank.labels..Zxyzw)rr()r-r?rrIrlabelsQs   $zSensorBank.labelsN) __name__ __module__ __qualname____doc__r3r7rBrErKrrrrrs   rc@s2eZdZdZdddZddZd d Zd d Zd S)AttitudeEstimatoraComplementary filter over the robot's own noisy gyro and accelerometer. A single accelerometer sample cannot tell you which way is up while the robot is moving: it measures gravity minus the body's own acceleration, and a balancing machine is always accelerating. Measured on this robot, tilt inferred from one sample is out by 25 degrees at the median, on a machine that falls at 55. The gyro has the opposite problem: excellent short term, drifts long term. Blending them is what every real balancing robot does on its MCU, so doing it here is modelling the hardware rather than cheating. The estimator sees only the same noisy, biased signals the policy does. \(\?{Gz?ףp= ?cCs,||_||_||_tgd|_d|_dSNr r r r )alpha heading_alphadtr*arrayupheading)r-rVrXrWrrrr3js zAttitudeEstimator.__init__cCstgd|_d|_dSrT)r*rYrZr[r-rrrr7qszAttitudeEstimator.resetc Cs|jt||j|j}tj|}|dkr6||n|j}tj|}|dkrt||}tddt|dd}d|j |}d||||} tj| }|dkr| |n||_n||_|jS)N& .>ư>r r gQ#@) rZr*crossrXlinalgnormr;maxabsrV) r-gyroaccelprednameastrustkZblendedrrrupdateus   zAttitudeEstimator.updatec CsD|jtt||j|j7_|dur0|jStj|td}tj|}|dkrX|jS||}||jt||j}tj|}|dkr|jS||}t |jt gd}tj|}|dkr|jS||}t ||j} t tt||tt|| } | |jt j dt j t j } |jd|j| 7_|jS)a5Yaw from the magnetometer, tilt-compensated, blended with the gyro. Gravity fixes roll and pitch but says nothing about yaw, so without a magnetometer heading drifts forever. The magnetometer is the only absolute yaw reference - and the least trustworthy sensor on the robot. Nr8r]r^)r r r r )r[r'r*dotrZrXr;r`rar_rYmathatan2pirW) r-rdmagmrgZhorizZhnZeastenZnorthrierrrrrupdate_headings."   $ z AttitudeEstimator.update_headingN)rQrRrS)rLrMrNrOr3r7rlrvrrrrrP[s  rPc@s(eZdZdZddZddZddZdS) FusedIMUaA BNO055/BNO085-class chip: fusion runs on the sensor, attitude comes out. Modelling the chip's OUTPUT rather than re-deriving it from raw gyro and accelerometer, because that is what the hardware actually provides. Honesty lives in the error model instead: white noise, a slow wander the chip cannot remove, a heading axis that is always worse than tilt, and extra heading error from six BLDC motors sitting near the sensor. c Cstj}||_||_|t|dd|_|t|dd|_|t|dd|_|t|dd|_ |t|d d |_ t d t t t|d d d||_|dS)NZtilt_noise_deg皙?Ztilt_drift_degr Zheading_noise_degg?Zheading_drift_deg@Zmotor_coupling_deg@rZ latency_ms g@@)roradiansrXrr'r tilt_noise tilt_drift head_noise head_driftmotor_couplingrbr)roundlatencyr7)r-cfgrXrdrrrr3s(zFusedIMU.__init__cCsB|jd|jd|_|jd|j|_td|_d|_ g|_ dS)NrrCr ) rr<r~ tilt_biasr head_biasr*r+walk head_walkqueuer\rrrr7s  zFusedIMU.resetcCsd|j|jd|jdd|_d|j|jd|jd|_tj|td}||j |j|jd|j d}tj |}|dkr||n t gd}||j|j|jd|j|j|}|j||ft|j|jkr|jdS|jdS)Ng+?r{Gz?rCr8r]rU)rrr<r~rrr*r;r'rr}r`rarYrrrrr(lenrpop)r-Zup_trueZ heading_truedriverZrgheadrrrrls" "  zFusedIMU.updateN)rLrMrNrOr3r7rlrrrrrws  rwc@sveZdZdddZddZd d Zd d Zd dZddZddZ ddZ ddZ ddZ ddZ ddZddZdS) RobotEnvrNr unionc& st|_tj|_|p2jdip,id}tj|||d_|_ jj _ t jdrnj ng_tjjd_tjj_tj_jjrbtjjd} tjjd} tdd| } tdd| } t| | \} }d td tjd | d td tjd |ddtd tjd| d|dtd tjd| d|d}|| t!dt"|!| }| d | d }}t!d| d}t#| D]|}t#| D]l}t$%|||||}|dkrddt$t$j|}|||f||||fd||||f<qܐq|&jj'dd<jd}dt"|dd_(t!dtt)j(jj*j+_,jdjdi}t"|dd_-t"|didd_.t$t$/t"|did d!_0tt)jj1j(_2|_3|j_4t5jd"i}|durZt6||d#<|_7jj8dddf9_:jj8dddf9_;t<jj=dddf9_>dj>j>dk<jj?_@gg_A_Bt5jd$id%i}tCjd&dr|DjjE|_F|GD]B\}}tHjtjIjJ|}jAKjjL|jBKt"|q"tjMjAtd'_AtMjB_BtjMfd(d)|Dtd'_Ndj:j;_Odj;j:_PtMd*d)jd+gD_QtRjd+gD]^\}}|d,d-krq||d.} | dur2qt"|d/d0}!| jO|<|!jP|<qSd1d)jd+gDtCjd+d}"|"rfd2d)|"Dn tTt#j@_UtVjU_@WtXjjj(j_Yjd3d4d5d6}#t6|#d#d4_Z|#dd7_[j[d8kr0t\|#j(j_]n(t^t"|#d9d5j(t"|#d:d;_]t_dgD_`tad?d=jd>gDd_bjZrd@nd_ci_ddAD]P}$tHjtjIje|$}%|%dkrtfjjg|%jjg|%jjh|%jd|$<qjYjijcjjjj@_kj@_lj@_ljjm9_njjo9_pjjq9_rjjs9_tjju9_vjjw9_xjj=9_yi_zdBD]F}$tHjtjIje|$}%tfjjg|%jjg|%jjh|%jz|$<qdS)CNtasktype)layout build_props)propsrr r g?rmg@rxg333333?g333333?g333333?g @g?g?g333333@r]ryg$@?simZ control_hzdrFlimitsZ target_height terminateZ min_heightZ max_tilt_deg7Zdomain_randomisationenabledinitqposposer8cs(g|] }jjtjtjj|qSr)r. jnt_dofadrr r!r" mjOBJ_JOINTrrgr\rrrJ7sz%RobotEnv.__init__..cSsg|]}|drdndqS)reversedr rrrhrrrrJCs actuatorsmodepositionjointZ action_scale333333?cSsg|] }|dqS)r rrrrrrJVrcsg|]}|qSr)indexr)namesrrrJXr estimatorTrQ)rrVZ complementaryfusedrVrWrScss&|]}|ddko|dVqdS)r rrrNrrxrrrrjsz$RobotEnv.__init__..rcss,|]$}|ddkrt|ddVqdS)r rrrr N)rr'rrrrrlsrdrerrgt_quatgt_posgt_upgt_velgt_gyro){rr/r*random default_rngrrrtask_objrr task_namehasattrrrrxmlr MjModelfrom_xml_stringr.MjDatadatanhfieldr) hfield_nrow hfield_ncollinspacemeshgridsinrqcosminrbr'rangerohypotravel hfield_datarropttimestepn_subh_targetmin_hr|min_up episode_s max_steps cmd_scale curriculumdictbooldractuator_ctrlrangecopyctrl_loctrl_hircactuator_forcerange tau_limitnun_act leg_joint_ids q_nominalgetattrrlrinit_qitemsr!r"rr( jnt_qposadrrYZ leg_dof_ids act_centeract_spanact_sign enumerate_settle_heightlistact_idxr_resolve_motorsrrest_onest_moderwrrPanyhas_magnext mag_couplingZest_dim_rawr#rr%r&r obs_extraZobs_dimact_dim body_mass _base_mass body_inertia _base_inertia body_ipos _base_ipos geom_friction_base_frictionactuator_gainprm _base_gainactuator_biasprm _base_bias_base_forcerange_gt)&r- spec_pathseed randomiserrtask_cfgrrnrZncZxsZysXYhZcxcyRrrHddwrrrrr valjidr@rhnominalspanwantZest_cfgnmr1r)rr-rr3s    "&&   4                    zRobotEnv.__init__c CsHt|j}|jdd|jdd<t|diD]6\}}t|jtjj |}|dkr4||j|jj |<q4t |j|d\}}t |jj D]d}|jj|tjjkrqt|jj|d}|dkrqt|j|d } |dus| |kr| |}}q|durt|jddd |_nt|jd |||_|j|_|jd |_dS) zWhere does the robot stand in THIS task's pose? Solved kinematically, not by simulating: an unsupported robot topples during any settle long enough to converge, and you end up measuring the height of a fallen robot. rootposNrCrr)Nr gQ?rmr)r rr.r/rrrr!r"rr mj_forwardrngeom geom_typemjtGeommjGEOM_CYLINDERr' geom_size geom_xposspawn_zrr) r-rr rrZlowestradiusgirzrrrrs,   zRobotEnv._settle_heightcCsddl}gii|_|_|_t|jdgD]p\}}t|j tj j |d}|j j ||j|<|ddkr0| |j|}|j||r|dnd|j|<q0dS) a Which actuators are torque-mode, and where their dof lives. This has to be re-resolved whenever the model changes. Swapping the root joint for a tether rig renumbers every dof - the free-floating robot has 14, hung from a hook it has 12 - so a cached dof index from the old model reads the wrong joint at best and runs off the end of qvel at worst. The torque-speed curve in step() dereferences these every tick, which is how a stale index crashed the whole live sim. rNrrrtorqueZ no_load_rad_sg@)mjcf torque_idxact_dofno_loadrr/rr r!r.r"rrZ motor_limitsr()r-_mjcfr@rhrZlimrrrrs  zRobotEnv._resolve_motorscsjjdddf_jjdddf_tjjdddf_djjdk<tj fddj Dt d_ tjjjj_i_d D]D}tjtjj|}tjj|jj|jj|j|<qi_d D]L}tjtjj|}|dkrtjj|jj|jj|j|<qjj_jj_jj _!jj"_#jj$_%jj&_'jj_(t)fd d t*jj+D _,dS) aRe-resolve everything that points into the model. Swapping the root joint (to build a tethered test rig) changes every index, so the sensor addresses and joint ids have to be looked up again or the readings silently belong to the wrong quantities. NrrFr r]cs(g|] }jjtjtjj|qSr)r.rr r!r"rrr\rrrJs  z$RobotEnv._rebind..r8rrc3s"|]}jj|tjjkVqdS)N)r.jnt_typer mjtJoint mjJNT_FREE)rjr\rrrsz#RobotEnv._rebind..)-r.rrrrr*rcrrrYrr)rrrr/rrrr r r!r"r#rr%r&rrrrrrrrrrrrrr rrnjnttethered)r-rr1rr\r_rebindsD       zRobotEnv._rebindcCsn|j}|j|jdd<|j|jdd<|j|jdd<|j|jdd<|j |j dd<|j |j dd<|j |jdd<d|_|jddsdS|jdddg\}}|jj|||jd}|j||jdd<|j|dddf|jdd<t|jdd}|rBt|tjj|jd d }|j||j| |d |j|<|jd ddg\}}|jdddf|j|||jdddf<|jd ddg\}}|jj|||jd}|j dddf||j dddf<|j dddf||j dddf<|j dddf||j dddf<|j |dddf|jdd<|jdddg\}}t|j||d|_dS)NrrFZ mass_scalerFr4Z com_offsetr rr rCZfriction_scaleZactuator_gain_scalermZ latency_steps)r.rrrrrrrrrrrrr rrrrrr6nbodyr'r r!r" mjOBJ_BODYr/rr)Zintegers)r-rslohircomZroot_bidgrrr _apply_drs: ".$$$ zRobotEnv._apply_drc stjjtjdidi}tj ddrN| j j jd}tddrdjj dd<t jjD]<}jj|tjjkrjj|}gdjj ||d <qjD]0\}}tjtjj|}|jj jj|<qtjjjjtgd _d_d _ d_!dddddddd _"d_#t$j%_&fd dt dD_'d _()j j*d_+,S|djj d d<j-j*.d |ddjj d<|dd}tj*.d |d|j*.d |d|j*.d |d|g} t$d } t/| | d| jj dd<|dd} t0j1j2D]$\} }|j*.d | jj | <q|j*.d |dddjj3dd<tjjjjtgd _d_j*j.dd} | tj45| d_6dddddddd _"d_#d _ d_!t$j%_&fd dt j(dD_')j7j _8j j*t9j:d!id"d_+,S)#Nrr init_noiserr6Fr )r r r r rUr)up_pkfa_ifa_pkside_iside_pkjerk_irate_pkcsg|]}tjqSrr*r+rr_r\rrrJ:rz"RobotEnv.reset..rFr rrCbase_zrmbase_rpy base_roll base_pitchbase_yawZxyzrbase_velr4r]csg|]}tjqSrrHrIr\rrrJbrpushevery_s);r>r mj_resetDatar.rrr/rrrrlr?rrr5r1r2 mjJNT_BALLrrrr!r"rr rr7rr*rYest_up est_headingstep_ir>feel _feel_prev_ar+r prev_action action_queuer _cache_truthr _next_push_obsr'r<mju_euler2Quatziprrqvelr`ra _mag_axisrrr'r)r-r rr4rhr rrrLZrpyquatjnr2vrr\rr7s   $   $   zRobotEnv.resetcCs|jj}tj||jdtd|_tj||jdtd|_tj||jdtd|_tj||jdtd|_ t||jdd|_ t|jd|_ dS) zpGround truth, refreshed once per step. Reward and scenery logic may read these; the observation may not.rr8rrrrrmN) rr=r*r;r r'base_pos base_quat vel_local gyro_trueup_zheight)r-sdrrrr^iszRobotEnv._cache_truthc Cst|jstdS|jdkr|j}td|d|d|d|ddd|d|d|d|d}ttt |j j d}|j tj|j j|jdtd||\}}|||_|_|S|jd |j j|j}|jd |j j|j}|j |||jrl|jd |j j|j}|jr^ttt |j j }||j|j|d }|j |||j jS) z7Produce the attitude the flight computer would receive.rrrmrCrFrzrr8rdrerrr)rr*r+rrirorpr'meanrcractuator_forcerrlr;r=r rWrXrrErrrrrdrvrZ) r-qyawrrZrr=rhrsrrr _est_updatets0  &&zRobotEnv._est_updatec Cs8|jd|jj|j}|jd|jj|j}t|dtgd}t t |d t dt |d}d}t |dt |d t |d}}}|t ||t ||d } |t ||t ||} ||} |j} td urTtd } | d urFtd dt| | dt| | d dt| | dtd <| | | ftd <t | d| | d<| dt| |j7<t | dt| | d<| dt| |j7<t | dt| | d<|j}|d ur| dt tt||d7<tj|t d|_t | dt t t|| d<d S)aTHE FEELINGS: the vestibular sense, metered every tick. A kid learns to stand by feeling its balance - these are the numbers that feeling is made of, taken from the same noisy IMU the real robot will carry (never ground truth). Every episode of every stage accumulates them in env.feel; a stage prices them into its score via FEEL_WEIGHTS when its exam wants the skill smooth, not just successful. up_pk peak vertical thrust (g, above gravity) fa_i/pk fore-aft g: integral and peak (the lurch) side_i/pk lateral g: integral and peak (the wobble) jerk_i integral of accel change (the roughness) rate_pk peak body rotation rate (rad/s) rerdrWrUrr^rmgfffffX@rFr NZ_prevZroughr rrArBrCrDrErFr8rG)rrErr=rrr*rYrorpr'rbrrrZ FEEL_ACCUMrrcrr[r,r;r)r-rhr=rZpZ_GaxZayazZup_gZfa_gZside_gfZ_pprevrrr _feel_updates>$($      (zRobotEnv._feel_updatecCs|j|jj|j}|jr\t|dtgd}t|dd}t |t |t |gg}n t d}t |||j||jgtjS)NrWrUrXr r)rrBrr=r>rrr*rY concatenaterorrr+robsr\astypefloat64)r-r0rZrZestrrrr`s   z RobotEnv._obscCsttj|tddd}|j||jd}tt|j }|||j <||j }t|j ||j |j |j}|jD]\}tt|jj|j|}|j|tdd||j|}tt||| |||<qx||jjdt|<|jdi}|jdr|j|jkr|jj|dddg} |jdd tj} |jjd| t| 7<|jjd | t | 7<|j|d d |_t!|j"D]} t#$|j%|jq|j|j&7_|j'd 7_'|(|)|*|j+,||j|j+-||\} } |j.|j/kpF|j0|j1kpFt2|jj34 pF|j+5|}|j'|j6k}||_7| t8||j0|j.t8|j+9|d }|:t| t8|t8||fS)Nr8rr rr rSrimpulsermrFrTr )termsfellrmrlsuccess);r*clipr;r'r]r(rr+rrrrrrrr-rcrrcr.rbr/ctrlrrr>r_rr6rorqrrrrr mj_stepr.rrYr^rsrzron_steprewardrlrrmrisfiniterall terminatedrr\rrr`)r-actionZappliedfullrr@rZavailrSimpangrJrrrZ truncatedinforrrstepsR     "   z RobotEnv.stepcsfddtdjjDS)Ncs g|]}tjtjj|qSr)r mj_id2namer.r"r9rr@r\rrrJsz'RobotEnv.body_names..rF)rr.r8r\rr\r body_namess  zRobotEnv.body_namescsntjdfddtdjjDfddtdjjDd}j}|durjdd|D|d <|S) zCBody world poses for the three.js bench, so the viewer needs no FK.r@cs"g|]}ddjj|DqS)cSsg|]}tt|dqSrrr'rrgrrrrJ r-RobotEnv.frame...)rxposrr\rrrJ sz"RobotEnv.frame..rFcs"g|]}ddjj|DqS)cSsg|]}tt|dqSrrrrrrrJrr)rxquatrr\rrrJs)r>rreNcSsg|]}t|dqS)r@)rrrrrrJrgoal)rr>rr.r8r goal_marker)r-rxrrr\rframe s      zRobotEnv.frame)rNr NNr)rLrMrNr3rrr7r>r7r^rsrzr`rrrrrrrrs  -"/.J 5 5r)rOronumpyr*r r,rrrtasksrrtrrPrwrrrrrs  DR2