"""Ambiente di simulazione di Pulcino (puro numpy + mujoco, nessuna dipendenza da gym). Contratto (SPEC §5.2): - controllo a 50 Hz (un passo = 20 ms = 5 passi di fisica da 4 ms) - osservazione (16): gravità nel frame corpo (3) | giroscopio rad/s (3) | ultima azione (6) | comando vx, wz (2) | sin, cos della fase del clock di passo a 1.6 Hz (2) - azione (6) a ∈ [-1, 1]: q_target = POSE_STAND + a · ACTION_SCALE q_cmd = ALPHA · q_target + (1 − ALPHA) · q_cmd_precedente (ALPHA = 0.6) poi clip ai limiti dei giunti - la politica NON vede gli angoli reali dei giunti: solo IMU, le proprie azioni, comando e clock. Modello servo MG90S "senza feedback" (quello che il robot vero fa davvero): - latenza di comando randomizzata 10–30 ms (per episodio) - setpoint interno che insegue il comando a velocità max 8 rad/s - banda morta ~0.017 rad (il servo non spinge se l'errore è più piccolo) - coppia max 0.2 N·m e forza contro-elettromotrice (nell'MJCF) - offset di calibrazione ±3° per servo (il robot vero non è mai calibrato perfettamente) Domain randomization (per episodio): massa dei corpi ±15%, attrito 0.5–1.2, offset servo ±3°, latenza, coppia max ±10% (batteria), guadagno kp ±20%, rumore e bias dell'IMU (anche un piccolo disallineamento di montaggio), spinte casuali sul torso. Uso rapido: env = PulcinoEnv(seed=0) obs = env.reset() obs, r, done, info = env.step(np.zeros(6)) """ import os import time from collections import deque import mujoco import numpy as np import os import time import xml.etree.ElementTree as ET QUI = os.path.dirname(os.path.abspath(__file__)) XML = os.path.join(QUI, "pulcino.xml") # ------------------------- costanti del contratto (SPEC) ------------------------- NOMI_GIUNTI = ["L_hip_roll", "L_hip_pitch", "L_ankle_pitch", "R_hip_roll", "R_hip_pitch", "R_ankle_pitch"] N_OBS, N_ACT = 16, 6 CTRL_HZ = 50.0 DT_CTRL = 1.0 / CTRL_HZ POSE_STAND = np.zeros(6) ACTION_SCALE = np.array([0.3, 0.6, 0.6, 0.3, 0.6, 0.6]) ALPHA = 0.6 # peso della nuova azione nel filtro passa-basso CLOCK_HZ = 1.6 # frequenza del clock di passo osservato dalla politica Q_MIN = np.array([-0.35, -0.8, -0.8, -0.35, -0.8, -0.8]) Q_MAX = -Q_MIN # ------------------------- modello servo MG90S ------------------------- SERVO_VMAX = 8.0 # rad/s SERVO_DEADBAND = 0.017 # rad SERVO_TMAX = 0.2 # N·m LATENZA_MIN, LATENZA_MAX = 0.010, 0.030 # s # ------------------------- scala dei comandi ------------------------- VX_MAX = 0.08 # m/s quando vx = 1 (stima prudente: gambe da 68 mm senza ginocchio) WZ_MAX = 0.8 # rad/s quando wz = 1 def _quat_to_mat(q): m = np.empty(9) mujoco.mju_quat2Mat(m, q) return m.reshape(3, 3) _CPU_FRACTION = float(os.environ.get('PULCINO_CPU_FRACTION','1')) _BUDGET_WALL = time.monotonic() _BUDGET_CPU = time.process_time() def _cpu_budget(): global _BUDGET_WALL, _BUDGET_CPU if _CPU_FRACTION >= 1: return elapsed_cpu=time.process_time()-_BUDGET_CPU if elapsed_cpu < .04: return delay=elapsed_cpu/max(.1,_CPU_FRACTION)-(time.monotonic()-_BUDGET_WALL) if delay>0: time.sleep(delay) _BUDGET_WALL=time.monotonic();_BUDGET_CPU=time.process_time() class PulcinoEnv: """Ambiente Pulcino. Tutta la randomizzazione è governata da `seed` (riproducibile).""" def __init__(self, seed=None, randomize=True, episode_s=10.0, cmd_mode="avanti", pushes=True, peso_clock=0.3, xml_path=XML): """peso_clock: peso del termine che premia i contatti dei piedi in fase col clock a 1.6 Hz (0 per il CPG, che ha una sua frequenza).""" level = os.environ.get('PULCINO_LEVEL') self.level = int(level) if level is not None else None if self.level is not None and not 0 <= self.level < 8: raise ValueError('Livello fuori intervallo') self.terrain_mode = 'mixed' if self.level is not None else os.environ.get('PULCINO_TERRAIN','flat') if self.terrain_mode == 'mixed': tree = ET.parse(xml_path); world = tree.getroot().find('worldbody') floor = world.find("geom[@name='floor']"); world.remove(floor) groundbody = ET.SubElement(world,'body',name='floorbody',mocap='true') groundbody.append(floor) for i, (x, y) in enumerate(((x*.04,y*.04) for x in range(-8,16) for y in range(-8,9))): tile=ET.SubElement(world,'body',name=f'terrainbody_{i}',mocap='true',pos=f'{x} {y} -0.02') ET.SubElement(tile,'geom',name=f'terrain_{i}',type='box',size='0.02 0.02 0.0025',contype='1',conaffinity='1',rgba='.3 .4 .5 1') for i in range(4): obstacle=ET.SubElement(world,'body',name=f'obstaclebody_{i}',mocap='true',pos='0 0 -0.1') ET.SubElement(obstacle,'geom',name=f'obstacle_{i}',type='box',size='0.015 0.02 0.025',contype='1',conaffinity='1',rgba='.8 .4 .2 1') from banana import add_banana add_banana(tree.getroot()) self.model=mujoco.MjModel.from_xml_string(ET.tostring(tree.getroot(),encoding='unicode')) else: self.model=mujoco.MjModel.from_xml_path(xml_path) self.data = mujoco.MjData(self.model) self.rng = np.random.default_rng(seed) self.randomize = randomize self.pushes = pushes self.peso_clock = peso_clock self.cmd_mode = cmd_mode self.max_steps = int(round(episode_s * CTRL_HZ)) m = self.model self.n_sub = int(round(DT_CTRL / m.opt.timestep)) self.dt_sub = m.opt.timestep # indici utili self.qadr = np.array([m.jnt_qposadr[m.joint(n).id] for n in NOMI_GIUNTI]) self.vadr = np.array([m.jnt_dofadr[m.joint(n).id] for n in NOMI_GIUNTI]) self.torso = m.body("torso").id self.g_lfoot, self.g_rfoot = m.geom("L_foot").id, m.geom("R_foot").id self.g_floor = m.geom("floor").id self.terrain_ids = [m.geom(f'terrain_{i}').id for i in range(408)] if self.terrain_mode == 'mixed' else [] self.floor_mocap = m.body_mocapid[m.body('floorbody').id] if self.terrain_mode == 'mixed' else None self.tile_mocaps = [m.body_mocapid[m.body(f'terrainbody_{i}').id] for i in range(408)] if self.terrain_mode == 'mixed' else [] self.obstacle_mocaps = [m.body_mocapid[m.body(f'obstaclebody_{i}').id] for i in range(4)] if self.terrain_mode == 'mixed' else [] self.ground_ids = {self.g_floor, *self.terrain_ids} self.gyro_adr = m.sensor_adr[m.sensor("gyro").id] # valori nominali da cui partire a ogni randomizzazione self.mass0 = m.body_mass.copy() self.inertia0 = m.body_inertia.copy() self.friction0 = m.geom_friction.copy() self.kp0 = m.actuator_gainprm[:, 0].copy() self.biasprm0 = m.actuator_biasprm.copy() self.frange0 = m.actuator_forcerange.copy() self.reset() # ------------------------------------------------------------------ def _randomizza(self): m, r = self.model, self.rng m.body_mass[:] = self.mass0 m.body_inertia[:] = self.inertia0 m.geom_friction[:] = self.friction0 m.actuator_gainprm[:, 0] = self.kp0 m.actuator_biasprm[:] = self.biasprm0 m.actuator_forcerange[:] = self.frange0 self.floor_quat = np.array([1.,0.,0.,0.]) self.terrain_heights = None; self.obstacles = [] self.scenario = 'piano'; self.terrain_lift = 0.0 terrain = slope = obstacles = False if self.level is not None: terrain = self.level % 4 in (1,3); obstacles = self.level % 4 in (2,3); slope = self.level >= 4 from curriculum import LEVELS self.scenario = LEVELS[self.level] elif self.terrain_mode == 'mixed' and self.randomize: self.scenario = r.choice(['piano','scivoloso','pendenza','rilievi']) terrain = self.scenario == 'rilievi'; slope = self.scenario == 'pendenza' if slope: axis = np.array([r.uniform(-1,1),r.uniform(-1,1),0.]); axis /= np.linalg.norm(axis) mujoco.mju_axisAngle2Quat(self.floor_quat,axis,np.deg2rad(r.uniform(-3,3))) self.terrain_lift = 0.006 if terrain: self.terrain_heights = r.uniform(0.0005,0.005,size=len(self.terrain_ids)) self.terrain_lift += 0.006 if obstacles: self.obstacles = [np.array([r.uniform(.075,.15), r.choice([-1,1])*r.uniform(.035,.075)]) for _ in range(4)] self.offset = np.zeros(6) self.latenza = 0.5 * (LATENZA_MIN + LATENZA_MAX) self.imu_rot = np.eye(3) self.gyro_bias = np.zeros(3) self.rumore_g, self.rumore_w = 0.0, 0.0 if not self.randomize: return s = r.uniform(0.85, 1.15, size=m.nbody) # massa ±15% s[0] = 1.0 # (il mondo non ha massa) m.body_mass[:] = self.mass0 * s m.body_inertia[:] = self.inertia0 * s[:, None] mu = r.uniform(0.25,0.5) if self.scenario == 'scivoloso' else r.uniform(0.5, 1.2) # attrito piedi/pavimento for g in (self.g_lfoot, self.g_rfoot, *self.ground_ids): m.geom_friction[g, 0] = mu kp = self.kp0 * r.uniform(0.8, 1.2, size=6) # rigidezza servo ±20% m.actuator_gainprm[:, 0] = kp m.actuator_biasprm[:, 1] = -kp # position: bias = -kp·q - kv·qdot m.actuator_forcerange[:] = self.frange0 * r.uniform(0.9, 1.1) # batteria self.offset = np.deg2rad(r.uniform(-3.0, 3.0, size=6)) # calibrazione ±3° self.latenza = r.uniform(LATENZA_MIN, LATENZA_MAX) # IMU: montaggio storto fino a ~2°, bias giroscopio, rumore ax = r.normal(size=3); ax /= np.linalg.norm(ax) ang = np.deg2rad(r.uniform(0, 2.0)) q = np.zeros(4) mujoco.mju_axisAngle2Quat(q, ax, ang) self.imu_rot = _quat_to_mat(q) self.gyro_bias = r.normal(0, 0.02, size=3) self.rumore_g = 0.02 self.rumore_w = 0.05 def _campiona_comando(self): r = self.rng if self.cmd_mode == "fermo": return np.zeros(2) if self.cmd_mode == "avanti": vx = r.uniform(0.3, 1.0) wz = 0.0 if r.random() < 0.7 else r.uniform(-0.5, 0.5) return np.array([vx, wz]) # "tutti": tutto lo spazio dei comandi, con un 10% di "stai fermo" if r.random() < 0.1: return np.zeros(2) return np.array([r.uniform(-1, 1), r.uniform(-1, 1)]) def reset(self, seed=None, cmd=None): if seed is not None: self.rng = np.random.default_rng(seed) self._randomizza() m, d = self.model, self.data mujoco.mj_resetData(m, d) # piccola perturbazione della posa iniziale if self.randomize: d.qpos[self.qadr] = self.rng.uniform(-0.03, 0.03, size=6) d.qpos[2] += 0.002 if self.terrain_heights is not None: d.mocap_pos[self.tile_mocaps,2] = self.terrain_heights - 0.0025 if self.floor_mocap is not None: d.mocap_quat[self.floor_mocap] = self.floor_quat # Il rilievo segue il piano inclinato, non resta orizzontale. slope_matrix = _quat_to_mat(self.floor_quat) if self.terrain_heights is not None: for index in self.tile_mocaps: d.mocap_pos[index] = slope_matrix @ d.mocap_pos[index] d.mocap_quat[index] = self.floor_quat for i, pos in enumerate(self.obstacles): index = self.obstacle_mocaps[i] d.mocap_pos[index] = slope_matrix @ np.array([*pos,.025]) d.mocap_quat[index] = self.floor_quat d.qpos[2] += self.terrain_lift mujoco.mj_forward(m, d) self.cmd = np.asarray(cmd, float) if cmd is not None else self._campiona_comando() self.goal = None if self.level is not None: from curriculum import GOAL_DISTANCE angle = self.rng.uniform(-.15,.15) self.goal = np.array([GOAL_DISTANCE*np.cos(angle),GOAL_DISTANCE*np.sin(angle)]) self.goal_previous = float(np.linalg.norm(self.goal-d.xpos[self.torso,:2])) self.t = 0.0 self.k = 0 self.a_prev = np.zeros(6) self.q_cmd = POSE_STAND.copy() # uscita del filtro passa-basso self.setpoint = d.qpos[self.qadr].copy() # setpoint interno del servo self.coda = deque() # comandi "in volo" (latenza) self.target_servo = self.setpoint.copy() self.push_t = self.rng.uniform(1.0, 3.0) self.push_fine = -1.0 self.pos_prec = d.xpos[self.torso].copy() self.yaw_prec = self._yaw() return self._osserva() # ------------------------------------------------------------------ def _yaw(self): R = self.data.xmat[self.torso].reshape(3, 3) return np.arctan2(R[1, 0], R[0, 0]) def gravita_corpo(self, rumore=True): """Gravità (versore) nel frame del corpo, come la stima dall'accelerometro dell'IMU.""" R = self.data.xmat[self.torso].reshape(3, 3) g = R.T @ np.array([0.0, 0.0, -1.0]) g = self.imu_rot @ g if rumore and self.rumore_g > 0: g = g + self.rng.normal(0, self.rumore_g, size=3) return g def giroscopio(self, rumore=True): w = self.data.sensordata[self.gyro_adr:self.gyro_adr + 3].copy() w = self.imu_rot @ w + self.gyro_bias if rumore and self.rumore_w > 0: w = w + self.rng.normal(0, self.rumore_w, size=3) return w def imu_roll_pitch(self, g=None): """Angoli roll/pitch dalla gravità nel corpo (regola della mano destra sugli assi x, y: roll > 0 = fianco sinistro in su, pitch > 0 = muso in giù).""" if g is None: g = self.gravita_corpo() roll = np.arctan2(-g[1], -g[2]) pitch = np.arctan2(g[0], -g[2]) return roll, pitch def _osserva(self): if self.goal is not None: pos = self.data.xpos[self.torso,:2] vector = self.goal-pos # Navigatore geometrico locale; la politica apprende il controllo arti. for obstacle in self.obstacles: delta=pos-obstacle; distance=np.linalg.norm(delta) if 0 < distance < .08: vector += delta/distance*(.08-distance)*1.2 error = np.arctan2(np.sin(np.arctan2(vector[1],vector[0])-self._yaw()),np.cos(np.arctan2(vector[1],vector[0])-self._yaw())) self.cmd=np.array([max(.15,np.cos(error))*.65, np.clip(error*1.5,-1,1)]) ph = 2 * np.pi * CLOCK_HZ * self.t self.g_last = self.gravita_corpo() return np.concatenate([self.g_last, self.giroscopio(), self.a_prev, self.cmd, [np.sin(ph), np.cos(ph)]]) # ------------------------------------------------------------------ def _servo_e_fisica(self, q_cmd): """Invia q_cmd ai servo (con latenza) e fa avanzare la fisica di un passo di controllo. Restituisce l'energia elettrica-meccanica spesa (J, approssimata con |τ·ω|).""" m, d = self.model, self.data self.coda.append((self.t + self.latenza, q_cmd + self.offset)) energia = 0.0 max_passo = SERVO_VMAX * self.dt_sub for _ in range(self.n_sub): tt = d.time while self.coda and self.coda[0][0] <= tt + 1e-9: self.target_servo = self.coda.popleft()[1] # il setpoint interno insegue il target a velocità limitata self.setpoint += np.clip(self.target_servo - self.setpoint, -max_passo, max_passo) sp = np.clip(self.setpoint, Q_MIN, Q_MAX) # banda morta: il servo non spinge se l'errore (misurato dal SUO potenziometro) è piccolo q = d.qpos[self.qadr] err = sp - q err_db = np.sign(err) * np.maximum(np.abs(err) - SERVO_DEADBAND, 0.0) d.ctrl[:] = q + err_db # spinte casuali sul torso if self.pushes and self.randomize: if tt >= self.push_t and self.push_fine < 0: f = self.rng.uniform(0.2, 0.6) a = self.rng.uniform(0, 2 * np.pi) d.xfrc_applied[self.torso, :3] = [f * np.cos(a), f * np.sin(a), 0.0] self.push_fine = tt + 0.1 if self.push_fine > 0 and tt >= self.push_fine: d.xfrc_applied[self.torso, :3] = 0.0 self.push_fine = -1.0 self.push_t = tt + self.rng.uniform(2.0, 4.0) mujoco.mj_step(m, d) energia += np.sum(np.abs(d.actuator_force * d.qvel[self.vadr])) * self.dt_sub self.t += DT_CTRL self.k += 1 return energia def _contatti_piedi(self): sx = dx = False d = self.data for i in range(d.ncon): c = d.contact[i] g = (c.geom1, c.geom2) if any(gid in self.ground_ids for gid in g): if self.g_lfoot in g: sx = True elif self.g_rfoot in g: dx = True return sx, dx def _ricompensa(self, a, energia): d = self.data pos = d.xpos[self.torso] yaw = self._yaw() v_mondo = (pos - self.pos_prec) / DT_CTRL # velocità nel frame "di direzione" (solo yaw) c, s = np.cos(yaw), np.sin(yaw) vx = c * v_mondo[0] + s * v_mondo[1] vy = -s * v_mondo[0] + c * v_mondo[1] wz = np.arctan2(np.sin(yaw - self.yaw_prec), np.cos(yaw - self.yaw_prec)) / DT_CTRL self.pos_prec = pos.copy() self.yaw_prec = yaw g = self.gravita_corpo(rumore=False) vx_t, wz_t = self.cmd[0] * VX_MAX, self.cmd[1] * WZ_MAX r_vx = np.exp(-((vx - vx_t) / 0.08) ** 2) # segue la velocità richiesta # progresso nella direzione richiesta (lineare, fino al target): dà un "gradiente" anche # quando il robot è ancora lontano dal target, così stare fermi non conviene r_prog = float(np.clip(vx / vx_t, -1.0, 1.0)) if abs(vx_t) > 1e-3 else 0.0 r_wz = np.exp(-((wz - wz_t) / 0.4) ** 2) # segue la rotazione richiesta p_vy = vy ** 2 / 0.05 ** 2 # niente derapate laterali p_tilt = g[0] ** 2 + g[1] ** 2 # resta dritto p_energia = energia / DT_CTRL # potenza media (W) p_fluido = np.sum((a - self.a_prev) ** 2) # niente scatti # piedi incrociati (in sim i piedi non collidono tra loro, sul robot sì) yl = d.geom_xpos[self.g_lfoot]; yr = d.geom_xpos[self.g_rfoot] dl = -s * (yl[0] - yr[0]) + c * (yl[1] - yr[1]) p_incrocio = 1.0 if dl < 0.040 else 0.0 # clock di passo: piede sinistro in aria nella prima metà del ciclo, destro nella seconda sx, dx = self._contatti_piedi() ph = np.sin(2 * np.pi * CLOCK_HZ * self.t) r_clock = 0.0 if abs(self.cmd[0]) > 0.05 or abs(self.cmd[1]) > 0.05: if ph > 0.3: r_clock = (0.5 if not sx else 0.0) + (0.5 if dx else 0.0) elif ph < -0.3: r_clock = (0.5 if not dx else 0.0) + (0.5 if sx else 0.0) else: r_clock = 0.5 * (sx + dx) r = (0.1 # "sopravvivenza" (piccola: se è grande, stare fermi vince) + 0.5 * r_vx + 1.0 * r_prog + 0.3 * r_wz + self.peso_clock * r_clock - 0.1 * p_vy - 1.0 * p_tilt - 0.05 * p_energia - 0.05 * p_fluido - 0.5 * p_incrocio) info = dict(vx=vx, vy=vy, wz=wz, tilt=float(np.sqrt(p_tilt)), potenza=p_energia) return r, info, g def _caduto(self, g): return g[2] > -0.5 or self.data.xpos[self.torso, 2] < 0.08 # inclinazione > 60° # ------------------------------------------------------------------ def azione_a_q(self, a): """Azione della politica → comando ai giunti (identico al firmware).""" a = np.clip(a, -1.0, 1.0) q_target = POSE_STAND + a * ACTION_SCALE self.q_cmd = ALPHA * q_target + (1.0 - ALPHA) * self.q_cmd return np.clip(self.q_cmd, Q_MIN, Q_MAX) def step(self, a): """Un passo a 50 Hz con l'azione della politica a ∈ [-1,1]^6.""" a = np.clip(np.asarray(a, float), -1.0, 1.0) q = self.azione_a_q(a) return self._dopo_passo(a, q) def step_q(self, q): """Un passo a 50 Hz dando direttamente gli angoli target (usato dal CPG, niente filtro).""" q = np.clip(np.asarray(q, float), Q_MIN, Q_MAX) a = (q - POSE_STAND) / ACTION_SCALE # solo per la penalità di fluidità / osservazione return self._dopo_passo(np.clip(a, -1, 1), q) def _dopo_passo(self, a, q): _cpu_budget() energia = self._servo_e_fisica(q) r, info, g = self._ricompensa(a, energia) self.a_prev = a.copy() caduto = self._caduto(g) if caduto: r -= 10.0 success = False if self.goal is not None: from curriculum import GOAL_RADIUS distance = float(np.linalg.norm(self.goal-self.data.xpos[self.torso,:2])) feet = (self.data.geom_xpos[self.g_lfoot,:2]+self.data.geom_xpos[self.g_rfoot,:2])/2 success = bool(not caduto and np.linalg.norm(self.goal-feet)= self.max_steps info["caduto"] = caduto if os.environ.get("PULCINO_RENDER_FRAME"): from render_live import capture capture(self) return self._osserva(), float(r), done, info if __name__ == "__main__": # prova veloce: robot fermo in piedi con azione zero import time env = PulcinoEnv(seed=0, cmd_mode="fermo") obs = env.reset() t0 = time.time() tot, n = 0.0, 0 done = False while not done: obs, r, done, info = env.step(np.zeros(6)) tot += r; n += 1 dt = time.time() - t0 print(f"passi {n}, ricompensa totale {tot:.2f}, caduto={info['caduto']}, " f"altezza torso {env.data.xpos[env.torso, 2]:.3f} m, {n / dt:.0f} passi/s") print("obs:", np.round(obs, 3))