VictorMylle/RL-Sim-Framework
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactor: merge motor sysid into unified sysid module

Browse Source 
Unified the two separate sysid codepaths (motor-only and full-system)
into a single module that optimizes all 28 parameters jointly:

- 13 motor params (asymmetric gear, damping, friction, deadzone,
  Stribeck boost, action bias, filter tau, armature, ctrl_limit)
- 15 pendulum/arm params (mass, CoM, inertia, joint dynamics)

Key changes:
- Added stribeck_friction_boost, stribeck_vel, action_bias to
  ActuatorConfig (robot.py) and MJX runner
- Created shared src/sysid/preprocess.py (SG velocity recomputation)
- Rewrote src/sysid/rollout.py with unified MOTOR_PARAMS + PENDULUM_PARAMS
  spec and PARAM_SETS dict for flexible subset optimization
- Updated optimize.py, export.py, visualize.py to use unified params
  (removed all LOCKED_MOTOR_PARAMS references)
- Removed src/sysid/motor/ module and scripts/motor_sysid.py

Net: -1383 lines, zero code duplication between motor and full-system sysid.
This commit is contained in:
Victor Mylle
2026-03-28 16:48:22 +01:00
parent ca0e7b8b03
commit 5880997786
20 changed files with 700 additions and 2083 deletions
Download Patch File Download Diff File Expand all files Collapse all files

316
src/sysid/rollout.py
View File

@@ -7,12 +7,11 @@ the simulated trajectory for comparison with the real recording.
This module is the inner loop of the CMA-ES optimizer: it is called once
per candidate parameter vector per generation.
Motor parameters are **locked** from the motor-only sysid result.
The optimizer only fits
pendulum/arm inertial parameters, pendulum joint dynamics, and
``ctrl_limit``. The asymmetric motor model (deadzone, gear compensation,
Coulomb friction, viscous damping, quadratic drag, back-EMF) is applied
via ``ActuatorConfig.transform_ctrl()`` and ``compute_motor_force()``.
All motor + pendulum/arm parameters are optimised jointly from a single
full-system recording. The asymmetric motor model (deadzone, gear
compensation, Coulomb friction + Stribeck, viscous damping, action bias)
is applied via ``ActuatorConfig.transform_ctrl()`` and
``compute_motor_force()``.
"""
from __future__ import annotations
@@ -32,26 +31,6 @@ from src.runners.mujoco import ActuatorLimits, load_mujoco_model
from src.sysid._urdf import patch_link_inertials
# ── Locked motor parameters (from motor-only sysid) ─────────────────
# These are FIXED and not optimised. They come from the 12-param model
# in robot.yaml (from motor-only sysid, cost 0.862).
LOCKED_MOTOR_PARAMS: dict[str, float] = {
"actuator_gear_pos": 0.424182,
"actuator_gear_neg": 0.425031,
"actuator_filter_tau": 0.00503506,
"motor_damping_pos": 0.00202682,
"motor_damping_neg": 0.0146651,
"motor_armature": 0.00277342,
"motor_frictionloss_pos": 0.0573282,
"motor_frictionloss_neg": 0.0533549,
"viscous_quadratic": 0.000285329,
"back_emf_gain": 0.00675809,
"motor_deadzone_pos": 0.141291,
"motor_deadzone_neg": 0.0780148,
}
# ── Tunable parameter specification ──────────────────────────────────
@@ -66,16 +45,31 @@ class ParamSpec:
log_scale: bool = False # optimise in log-space (masses, inertias)
# Pendulum sysid parameters — motor params are LOCKED (not here).
# Order matters: the optimizer maps a flat vector to these specs.
# Defaults are from the URDF exported by Fusion 360.
ROTARY_CARTPOLE_PARAMS: list[ParamSpec] = [
# ── Arm link (URDF) ──────────────────────────────────────────
# ── Motor parameters (13) ────────────────────────────────────────────
# Defaults from motor-only sysid (cost 0.2117).
MOTOR_PARAMS: list[ParamSpec] = [
ParamSpec("actuator_gear_pos", 0.371194, 0.005, 1.5, log_scale=True),
ParamSpec("actuator_gear_neg", 0.428143, 0.005, 1.5, log_scale=True),
ParamSpec("actuator_filter_tau", 0.022301, 0.001, 0.20),
ParamSpec("motor_damping_pos", 0.001384, 1e-5, 0.1, log_scale=True),
ParamSpec("motor_damping_neg", 0.005196, 1e-5, 0.1, log_scale=True),
ParamSpec("motor_armature", 0.002753, 1e-6, 0.01, log_scale=True),
ParamSpec("motor_frictionloss_pos", 0.036744, 0.001, 0.2, log_scale=True),
ParamSpec("motor_frictionloss_neg", 0.069082, 0.001, 0.2, log_scale=True),
ParamSpec("stribeck_friction_boost", 0.0, 0.0, 0.15),
ParamSpec("stribeck_vel", 2.0, 0.1, 8.0),
ParamSpec("motor_deadzone_pos", 0.14182, 0.0, 0.30),
ParamSpec("motor_deadzone_neg", 0.031454, 0.0, 0.30),
ParamSpec("action_bias", 0.0, -0.10, 0.10),
]
# ── Pendulum/arm parameters (15) ─────────────────────────────────────
# Defaults from Fusion 360 URDF export.
PENDULUM_PARAMS: list[ParamSpec] = [
ParamSpec("arm_mass", 0.02110, 0.005, 0.08, log_scale=True),
ParamSpec("arm_com_x", -0.00710, -0.03, 0.03),
ParamSpec("arm_com_y", 0.00085, -0.02, 0.02),
ParamSpec("arm_com_z", 0.00795, -0.02, 0.02),
# ── Pendulum link (URDF) ─────────────────────────────────────
ParamSpec("pendulum_mass", 0.03937, 0.010, 0.10, log_scale=True),
ParamSpec("pendulum_com_x", 0.06025, 0.01, 0.15),
ParamSpec("pendulum_com_y", -0.07602, -0.20, 0.0),
@@ -84,37 +78,18 @@ ROTARY_CARTPOLE_PARAMS: list[ParamSpec] = [
ParamSpec("pendulum_iyy", 3.70e-05, 1e-07, 1e-03, log_scale=True),
ParamSpec("pendulum_izz", 7.83e-05, 1e-07, 1e-03, log_scale=True),
ParamSpec("pendulum_ixy", -6.93e-06, -1e-03, 1e-03),
# ── Pendulum joint dynamics ──────────────────────────────────
ParamSpec("pendulum_damping", 0.0001, 1e-6, 0.05, log_scale=True),
ParamSpec("pendulum_frictionloss", 0.0001, 1e-6, 0.05, log_scale=True),
# ── Hardware realism (control pipeline) ────────────────────
ParamSpec("ctrl_limit", 0.588, 0.45, 0.70), # MAX_MOTOR_SPEED / 255
]
# Extended set: full params + motor armature (compensates for the
# motor-only sysid having captured arm/pendulum loading in armature).
EXTENDED_PARAMS: list[ParamSpec] = ROTARY_CARTPOLE_PARAMS + [
ParamSpec("motor_armature", 0.00277, 0.0005, 0.02, log_scale=True),
]
# Reduced set: only the 6 most impactful pendulum parameters.
# Good for a fast first pass — converges in ~50 generations.
REDUCED_PARAMS: list[ParamSpec] = [
ParamSpec("pendulum_mass", 0.03937, 0.010, 0.10, log_scale=True),
ParamSpec("pendulum_com_x", 0.06025, 0.01, 0.15),
ParamSpec("pendulum_com_y", -0.07602, -0.20, 0.0),
ParamSpec("pendulum_ixx", 6.20e-05, 1e-07, 1e-03, log_scale=True),
ParamSpec("pendulum_damping", 0.0001, 1e-6, 0.05, log_scale=True),
ParamSpec("pendulum_frictionloss", 0.0001, 1e-6, 0.05, log_scale=True),
ParamSpec("ctrl_limit", 0.588, 0.45, 0.70),
]
# ── Combined parameter sets ──────────────────────────────────────────
ROTARY_CARTPOLE_PARAMS: list[ParamSpec] = MOTOR_PARAMS + PENDULUM_PARAMS
PARAM_SETS: dict[str, list[ParamSpec]] = {
"full": ROTARY_CARTPOLE_PARAMS,
"extended": EXTENDED_PARAMS,
"reduced": REDUCED_PARAMS,
"motor": MOTOR_PARAMS,
"pendulum": PENDULUM_PARAMS,
}
@@ -134,6 +109,13 @@ def defaults_vector(specs: list[ParamSpec] | None = None) -> np.ndarray:
return np.array([s.default for s in specs], dtype=np.float64)
def defaults_dict(specs: list[ParamSpec] | None = None) -> dict[str, float]:
"""Return the default parameter dict (all 28 params)."""
if specs is None:
specs = ROTARY_CARTPOLE_PARAMS
return {s.name: s.default for s in specs}
def bounds_arrays(
specs: list[ParamSpec] | None = None,
) -> tuple[np.ndarray, np.ndarray]:
@@ -148,19 +130,23 @@ def bounds_arrays(
# ── MuJoCo model building with parameter overrides ──────────────────
def _resolve_params(params: dict[str, float]) -> dict[str, float]:
"""Fill in defaults for any missing params."""
full = defaults_dict()
full.update(params)
return full
def _build_model(
robot_path: Path,
params: dict[str, float],
motor_params: dict[str, float] | None = None,
) -> tuple[mujoco.MjModel, ActuatorConfig]:
"""Build a MuJoCo model with sysid overrides.
Returns (model, actuator) — use ``actuator.transform_ctrl()`` and
``actuator.compute_motor_force()`` in the rollout loop.
"""
if motor_params is None:
motor_params = LOCKED_MOTOR_PARAMS
p = _resolve_params(params)
robot_path = Path(robot_path).resolve()
# ── Patch URDF inertial parameters to a temp file ────────────
@@ -168,7 +154,7 @@ def _build_model(
urdf_path = robot_path / robot_yaml["urdf"]
tree = ET.parse(urdf_path)
patch_link_inertials(tree.getroot(), params)
patch_link_inertials(tree.getroot(), p)
fd, tmp_urdf = tempfile.mkstemp(
suffix=".urdf", prefix="_sysid_", dir=str(robot_path),
@@ -177,39 +163,22 @@ def _build_model(
tmp_urdf_path = Path(tmp_urdf)
tree.write(str(tmp_urdf_path), xml_declaration=True, encoding="unicode")
# ── Build RobotConfig with full motor sysid values ───────────
gear_pos = motor_params.get("actuator_gear_pos", 0.424182)
gear_neg = motor_params.get("actuator_gear_neg", 0.425031)
motor_armature = params.get(
"motor_armature",
motor_params.get("motor_armature", 0.00277342),
)
pend_damping = params.get("pendulum_damping", 0.0001)
pend_frictionloss = params.get("pendulum_frictionloss", 0.0001)
# ── Build RobotConfig with full motor model ──────────────────
act_cfg = robot_yaml["actuators"][0]
ctrl_lo, ctrl_hi = act_cfg.get("ctrl_range", [-1.0, 1.0])
actuator = ActuatorConfig(
joint=act_cfg["joint"],
type="motor",
gear=(gear_pos, gear_neg),
ctrl_range=(ctrl_lo, ctrl_hi),
deadzone=(
motor_params.get("motor_deadzone_pos", 0.141),
motor_params.get("motor_deadzone_neg", 0.078),
),
damping=(
motor_params.get("motor_damping_pos", 0.002),
motor_params.get("motor_damping_neg", 0.015),
),
frictionloss=(
motor_params.get("motor_frictionloss_pos", 0.057),
motor_params.get("motor_frictionloss_neg", 0.053),
),
filter_tau=motor_params.get("actuator_filter_tau", 0.005),
viscous_quadratic=motor_params.get("viscous_quadratic", 0.000285),
back_emf_gain=motor_params.get("back_emf_gain", 0.00676),
gear=(p["actuator_gear_pos"], p["actuator_gear_neg"]),
ctrl_range=(act_cfg.get("ctrl_range", [-1.0, 1.0])[0],
act_cfg.get("ctrl_range", [-1.0, 1.0])[1]),
deadzone=(p["motor_deadzone_pos"], p["motor_deadzone_neg"]),
damping=(p["motor_damping_pos"], p["motor_damping_neg"]),
frictionloss=(p["motor_frictionloss_pos"], p["motor_frictionloss_neg"]),
stribeck_friction_boost=p["stribeck_friction_boost"],
stribeck_vel=p["stribeck_vel"],
action_bias=p["action_bias"],
filter_tau=p["actuator_filter_tau"],
)
robot = RobotConfig(
@@ -218,12 +187,12 @@ def _build_model(
joints={
"motor_joint": JointConfig(
damping=0.0,
armature=motor_armature,
armature=p["motor_armature"],
frictionloss=0.0,
),
"pendulum_joint": JointConfig(
damping=pend_damping,
frictionloss=pend_frictionloss,
damping=p["pendulum_damping"],
frictionloss=p["pendulum_frictionloss"],
),
},
)
@@ -236,7 +205,136 @@ def _build_model(
return model, actuator
def build_base_model(
robot_path: str | Path,
params: dict[str, float] | None = None,
) -> tuple[mujoco.MjModel, ActuatorConfig, dict[str, int], dict[str, int]]:
"""Build a base MuJoCo model once (with default or given params).
Returns (model, actuator, body_ids, dof_ids) where body_ids/dof_ids
map names to indices for fast in-place patching.
"""
if params is None:
params = defaults_dict()
model, actuator = _build_model(Path(robot_path).resolve(), params)
body_ids = {}
for name in ("arm", "pendulum"):
body_ids[name] = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name)
dof_ids = {}
for name in ("motor_joint", "pendulum_joint"):
jnt_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
dof_ids[name] = model.jnt_dofadr[jnt_id]
return model, actuator, body_ids, dof_ids
def _rotmat_to_quat(R: np.ndarray) -> np.ndarray:
"""Convert a 3×3 rotation matrix to a MuJoCo quaternion [w, x, y, z]."""
quat = np.empty(4)
mujoco.mju_mat2Quat(quat, R.flatten())
return quat
def _patch_pendulum_inertia(
model: mujoco.MjModel,
body_id: int,
params: dict[str, float],
) -> None:
"""Diagonalize pendulum inertia tensor and write to model."""
defs = defaults_dict()
ixx = params.get("pendulum_ixx", defs["pendulum_ixx"])
iyy = params.get("pendulum_iyy", defs["pendulum_iyy"])
izz = params.get("pendulum_izz", defs["pendulum_izz"])
ixy = params.get("pendulum_ixy", defs.get("pendulum_ixy", 0.0))
# Build full 3×3 inertia tensor (ixz = iyz = 0 in our URDF).
I = np.array([
[ixx, ixy, 0.0],
[ixy, iyy, 0.0],
[0.0, 0.0, izz],
])
# Eigendecompose — eigenvalues are principal moments, eigenvectors
# define the rotation from link frame to principal frame.
eigenvalues, eigenvectors = np.linalg.eigh(I)
# eigh returns ascending order; MuJoCo wants them in the order that
# the eigenvector matrix forms a proper rotation (det = +1).
if np.linalg.det(eigenvectors) < 0:
eigenvectors[:, 0] *= -1
# Clamp tiny/negative eigenvalues (numerical noise).
eigenvalues = np.maximum(eigenvalues, 1e-10)
# Enforce triangle inequality (required for valid rigid body inertia).
# For principal moments sorted ascending (a <= b <= c): a + b >= c.
# When violated, MuJoCo falls back to spherical inertia (trace/3).
# We must match this to keep fast path consistent with URDF loading.
s = np.sort(eigenvalues)
if s[0] + s[1] < s[2]:
sphere = np.sum(eigenvalues) / 3.0
eigenvalues[:] = sphere
eigenvectors = np.eye(3)
model.body_inertia[body_id] = eigenvalues
model.body_iquat[body_id] = _rotmat_to_quat(eigenvectors)
def patch_model(
model: mujoco.MjModel,
params: dict[str, float],
body_ids: dict[str, int],
dof_ids: dict[str, int],
) -> None:
"""Patch a cached MuJoCo model in-place with new sysid parameters.
Much faster than _build_model: no XML parsing, no temp files, no
model reload. Just direct array writes.
"""
p = _resolve_params(params)
# ── Arm body ─────────────────────────────────────────────────
arm_id = body_ids["arm"]
model.body_mass[arm_id] = p["arm_mass"]
for i, key in enumerate(("arm_com_x", "arm_com_y", "arm_com_z")):
model.body_ipos[arm_id, i] = p[key]
# ── Pendulum body ────────────────────────────────────────────
pend_id = body_ids["pendulum"]
model.body_mass[pend_id] = p["pendulum_mass"]
for i, key in enumerate(("pendulum_com_x", "pendulum_com_y", "pendulum_com_z")):
model.body_ipos[pend_id, i] = p[key]
_patch_pendulum_inertia(model, pend_id, p)
# ── Joint dynamics ───────────────────────────────────────────
motor_dof = dof_ids["motor_joint"]
model.dof_armature[motor_dof] = p["motor_armature"]
pend_dof = dof_ids["pendulum_joint"]
model.dof_damping[pend_dof] = p["pendulum_damping"]
model.dof_frictionloss[pend_dof] = p["pendulum_frictionloss"]
def _make_actuator(params: dict[str, float]) -> ActuatorConfig:
"""Build an ActuatorConfig from a params dict (for fast-path use)."""
p = _resolve_params(params)
return ActuatorConfig(
joint="motor_joint",
type="motor",
gear=(p["actuator_gear_pos"], p["actuator_gear_neg"]),
ctrl_range=(-1.0, 1.0),
deadzone=(p["motor_deadzone_pos"], p["motor_deadzone_neg"]),
damping=(p["motor_damping_pos"], p["motor_damping_neg"]),
frictionloss=(p["motor_frictionloss_pos"], p["motor_frictionloss_neg"]),
stribeck_friction_boost=p["stribeck_friction_boost"],
stribeck_vel=p["stribeck_vel"],
action_bias=p["action_bias"],
filter_tau=p["actuator_filter_tau"],
)
# ── Simulation rollout ───────────────────────────────────────────────
@@ -248,35 +346,30 @@ def rollout(
actions: np.ndarray,
sim_dt: float = 0.002,
substeps: int = 10,
motor_params: dict[str, float] | None = None,
) -> dict[str, np.ndarray]:
"""Replay recorded actions in MuJoCo with overridden parameters.
Parameters
----------
robot_path : asset directory
params : named parameter overrides (pendulum/arm only)
params : named parameter overrides (all motor + pendulum params)
actions : (N,) normalised actions [-1, 1] from the recording
sim_dt : MuJoCo physics timestep
substeps : physics substeps per control step
motor_params : locked motor params (default: LOCKED_MOTOR_PARAMS)
Returns
-------
dict with keys: motor_angle, motor_vel, pendulum_angle, pendulum_vel
Each is an (N,) numpy array of simulated values.
"""
if motor_params is None:
motor_params = LOCKED_MOTOR_PARAMS
robot_path = Path(robot_path).resolve()
model, actuator = _build_model(robot_path, params, motor_params)
model, actuator = _build_model(robot_path, params)
model.opt.timestep = sim_dt
data = mujoco.MjData(model)
mujoco.mj_resetData(model, data)
n = len(actions)
ctrl_limit = params.get("ctrl_limit", 0.588)
p = _resolve_params(params)
ctrl_limit = p["ctrl_limit"]
sim_motor_angle = np.zeros(n, dtype=np.float64)
sim_motor_vel = np.zeros(n, dtype=np.float64)
@@ -315,41 +408,27 @@ def windowed_rollout(
window_duration: float = 0.5,
sim_dt: float = 0.002,
substeps: int = 10,
motor_params: dict[str, float] | None = None,
) -> dict[str, np.ndarray | float]:
"""Multiple-shooting rollout — split recording into short windows.
For each window:
1. Initialize MuJoCo state from the real qpos/qvel at the window start.
2. Replay the recorded actions within the window.
3. Record the simulated output.
Motor dynamics (asymmetric friction, damping, back-EMF, etc.) are
applied via qfrc_applied using the locked motor sysid parameters.
Parameters
----------
robot_path : asset directory
params : named parameter overrides (pendulum/arm only)
params : named parameter overrides (all motor + pendulum params)
recording : dict with keys time, action, motor_angle, motor_vel,
pendulum_angle, pendulum_vel (all 1D arrays of length N)
window_duration : length of each shooting window in seconds
sim_dt : MuJoCo physics timestep
substeps : physics substeps per control step
motor_params : locked motor params (default: LOCKED_MOTOR_PARAMS)
Returns
-------
dict with:
motor_angle, motor_vel, pendulum_angle, pendulum_vel — (N,) arrays
(stitched from per-window simulations)
n_windows — number of windows used
"""
if motor_params is None:
motor_params = LOCKED_MOTOR_PARAMS
robot_path = Path(robot_path).resolve()
model, actuator = _build_model(robot_path, params, motor_params)
model, actuator = _build_model(robot_path, params)
model.opt.timestep = sim_dt
data = mujoco.MjData(model)
@@ -378,7 +457,8 @@ def windowed_rollout(
window_starts.append(idx)
current_t += window_duration
ctrl_limit = params.get("ctrl_limit", 0.588)
p = _resolve_params(params)
ctrl_limit = p["ctrl_limit"]
n_windows = len(window_starts)
for w, w_start in enumerate(window_starts):