# Stringman (Neufangled), "Arpeggio" layout, carbon 270 mm pole. Every number says where it came from.
# fw = github.com/nhnifong/cranebot3-firmware @ 370287a (2026-10-07). fw-sim = its simulator/ (MuJoCo model + bridge),
# whose own README lists which of its numbers are guesses. MEASURE = not in the docs or the firmware: a guess until
# measured on the real machine. Full table with reasoning: docs/MACHINE.md.

room:
  # fw-sim: a 4 m square room, anchors (2,2,2) and (-2,-2,2). Docs: anchors ~13 cm below the ceiling. We keep the
  # fw-sim footprint but put the mount points at 2.30 m (a 2.43 m ceiling, the common one): fw-sim's 2.0 m leaves the
  # gripper no reach to a high shelf (lines flatten, tension grows as 1/sin(angle)).
  side_m: 4.0
  mount_z_m: 2.30
  # corner mount points, xy, inset from the walls by the anchor's eyelet offset (fw definitions.arp_anchor_*_eyelet,
  # ~5 cm). Order = the firmware's line order: 0 anchor0 direct, 1 anchor0 indirect (via post A), 2 anchor1 direct,
  # 3 anchor1 indirect (via post B) (fw-sim tendons line_0..3).
  mounts_xy: [[1.95, 1.95], [-1.95, 1.95], [-1.95, -1.95], [1.95, -1.95]]
  indirect_run_m: [0.0, 3.93, 0.0, 3.93]   # fixed cross-room run of the indirect lines (fw-sim: 3.9301 m)

lines:
  # fw-sim: 4000 N/m per line at ~3 m (its README: "may be too stiff; real monofilament stretches considerably more").
  # We write it as EA = k * L so a longer line (indirect, + 3.93 m) is softer: EA = 4000 * 3.0. MEASURE.
  EA_N: 12000.0
  damping_Ns_m: 40.0            # fw-sim kv 80 + tendon damping 4; halved: kv acts on a slack line too. MEASURE
  max_force_N: 40.0             # fw spool_dm NO_CONN_TENSION_LIMIT 40 N (motor torque cut); fw-sim caps at 25 N
  max_safe_tension_N: 28.0      # fw config_loader default (16 N backfill on older configs; observer default 16)
  tension_floor_N: 0.1          # fw spool_dm TENSION_FLOOR_N: the spool reels in to keep this on every line
  tension_kp_mps_per_N: 0.3     # fw spool_dm TENSION_KP
  tension_corr_max_mps: 0.5     # fw spool_dm MAX_TENSION_CORRECTION_MPS

spool:
  loop_hz: 50                   # fw spool_dm LOOP_FREQ_HZ
  max_line_speed_mps: 2.0       # fw spool_dm MAX_SAFE_LINE_SPEED
  # Line acceleration. fw spool_dm MAX_ACCEL 0.01 and config max_accel 0.3 go to the DaMiao ACC register in units the
  # code does not state; fw-sim floors it at 5 rad/s^2 "so the simulator is usable". 5 rad/s^2 x 0.229 m/rev / 2pi
  # (72-73 mm spool, fw definitions.damiao_*) = 0.18 m/s^2 of line. MEASURE (step a line speed, log encoder).
  line_accel_mps2: 0.18
  meters_per_rev: 0.229         # pi x 73 mm (fw definitions.damiao_full_spool_diameter_fishing_line)

host:
  # fw observer.speed_limit(): 0.35 m/s (0.45 with firmware gate "speed_0.45"); the norm of the summed velocity.
  speed_limit_mps: 0.35
  velocity_key_ttl_s: 2.0       # fw observer INPUT_VELOCITY_TTL_S: a keyed velocity expires 2 s after its last update
  estimator_hz: 60              # fw position_estimator.predict_forwards (visual update 30 Hz, commanded-vel 30 Hz)
  # move_direction_speed adds downward_bias -0.04 to unit vectors; with speed=None (the LeRobot path, and ours) the
  # velocity is used as given. Line speeds come from finite differences at the ESTIMATED gantry position, once per
  # command (KINEMATICS_STEP_SCALE 10): our plant does the same every planner tick.

body:
  # Gantry + pole + gripper hang as ONE rigid body from the point where the 4 lines meet (fw-sim; fw swing.py treats it
  # as a pendulum). Masses: fw-sim estimates ("nothing in the firmware repo records them"). MEASURE (kitchen scale).
  gantry_mass_kg: 0.35
  pole_mass_kg: 0.045
  gripper_mass_kg: 0.60
  finger_mass_kg: 0.04
  hook_r_m: 0.055               # fw-sim (from gantry.glb): lines clip to cross arms 55 mm out, on the diagonals
  hook_z_m: 0.02
  pole_top_z_m: -0.06
  pole_bottom_z_m: -0.329       # fw-sim: pole_offset_carbon270 - grommet height 0.1167
  gripper_origin_z_m: -0.4457   # fw definitions.pole_offset_carbon270 (0.5757 - 0.13)
  gripper_cog_dz_m: 0.055       # fw definitions.gripper_cog (y-up gripper frame: 55 mm above the gripper origin)
  # Swing: fw definitions.pole_length_carbon270 = 0.3089 m effective (measured with 'polecal'): period 1.115 s. The
  # gripper inertia is solved at build time so the twin's swing period matches it (fw-sim is ~10 % long, its README).
  swing_length_m: 0.3089
  # swing damping on the body's rotation (N m s): zeta ~ 0.02 = c / (2 I_O omega), I_O ~ 0.12 kg m^2, omega 5.8 rad/s,
  # 1/e in ~9 s. fw-sim has none (its swing never decays); fw observer fits a decay during 'polecal'. MEASURE.
  swing_damping_Nms: 0.03
  finger_pivot_x_m: 0.025       # fw-sim: two fingers pivot at x = +-0.025, close side to side, geared on one servo
  finger_pivot_dz_m: 0.041      # pivot above the gripper origin (fw-sim, gripper.glb)
  finger_len_m: 0.110           # pivot to pad centre, down (fw-sim pad_left pos y -0.110)
  pad_inset_m: 0.019            # pad centre inside the pivot when closed (fw-sim pad pos x -0.019)
  pad_half: [0.006, 0.022, 0.030]   # fw-sim pad box half sizes (x across, y along, z vertical)
  finger_travel_deg: 59.0       # fw gripper_arp_server.FINGER_TRAVEL_DEG; firmware reports -90 (open)..90 (closed)
  camera_dy_m: 0.026            # fw definitions.gripper_camera: 27 mm toward the nose, tilted 9.06 deg back
  camera_dz_m: 0.0026
  camera_tilt_deg: 9.06
  camera_fovy_deg: 42.557       # fw-sim: 2 atan(192/493), cameraCalWide; stream 684x384 @ 54 fps
  camera_res: [684, 384]
  laser_dy_m: -0.050            # fw-sim rangefinder site (0, 0.0025, 0.0502) in the gripper frame
  laser_dz_m: 0.0025

gripper:
  loop_hz: 60                   # fw gripper_arp_server DT = 1/60
  wrist_range_deg: [0, 1080]    # docs python_api; fw-sim joint +-3 pi about the neutral heading
  wrist_max_dps: 180.0          # MEASURE: docs ease_wrist peak 90 deg/s default; servo ST3215 class ~ 300 deg/s
  finger_max_dps: 360.0         # MEASURE (firmware-degrees per s on the -90..90 scale, ~118 real deg/s)
  action_timeout_s: 0.2         # fw ACTION_TIMEOUT: wrist/finger speeds zero after 0.2 s without a command

# ----------------------------------------------------------------------------------------------- the hand (M3)
# Which hand hangs under the pole. stringman = the firmware gripper above (two fingers geared on one servo): M0-M2 and
# cdpr_wm_g1 were built on it. so101 = the SO-101's wrist_roll + gripper (two STS3215) and its wrist camera, for the
# pick-waste missions with real objects (2026-10-09). load_config(hand=...) overrides; so101 replaces `body`'s gripper
# numbers and `gripper`'s loop numbers with the ones below.
hand: stringman

so101:
  # Source: armpilot/third_party/so101/so101_new_calib_camera.xml (onshape-to-robot from the SO-101 CAD) unless noted;
  # geometry measured from its meshes in MuJoCo (2026-10-09). Hand frame = the SO-101 gripper link frame: origin on the
  # wrist_roll axis at the follower's top face, z up along that axis (= the pole), jaws close along x, camera at +y.
  hand_origin_z_m: -0.374       # MEASURE: pole bottom (-0.329) - 45 mm for the roll servo + adapter plate
  roll_servo_mass_kg: 0.079     # the SO-101 `wrist` link (roll servo + bracket): stands in for servo + adapter. MEASURE
  roll_servo_half: [0.0124, 0.0175, 0.0225]   # STS3215 45.2 x 24.7 x 35 mm (datasheet), shaft along the pole
  gripper_mass_kg: 0.087        # `gripper` link: gripper servo + fixed jaw (wrist_roll_follower)
  gripper_com: [0.000214, 0.000245, -0.025187]
  gripper_fullinertia: [2.75087e-05, 4.33657e-05, 3.45059e-05, -3.35241e-07, -5.7352e-06, -5.17847e-08]
  jaw_mass_kg: 0.012            # `moving_jaw_so101_v1`
  jaw_com: [-0.00157, -0.0193, -0.0300]       # its inertial, rotated into the hand frame (jaw link is +90 deg about x)
  jaw_diaginertia: [6.61e-06, 5.29e-06, 1.89e-06]
  camera_mass_kg: 0.035         # wrist_camera_mount 0.020 + wrist_camera 0.015 (32x32 UVC module)
  # gripper servo body (mesh bounds): x -0.015..0.0304, y -0.0193..0.0203, z -0.0358..-0.011
  servo_box: {pos: [0.0077, 0.0005, -0.0234], half: [0.0227, 0.0198, 0.0124]}
  # fixed jaw finger: inner face x = -0.0079 (the lerobot `gripperframe` site sits on it at z -0.098), tip z -0.1044
  fixed_jaw: {x: [-0.0235, -0.0079], y: [-0.008, 0.008], z: [-0.1044, -0.011]}
  # moving jaw: hinge at the anchor about hand -y (SO-101 `gripper` joint); finger in the hand frame at q = 0 relative to
  # the anchor; inner face 15.8 mm from the fixed jaw's at q = 0, 1.9 mm at the joint's lower limit -10 deg (the pads,
  # 1 mm each here, meet there)
  jaw_anchor: [0.0202, 0.0188, -0.0234]
  moving_jaw: {x: [-0.0123, 0.0045], y: [-0.0266, -0.0110], z: [-0.082, 0.010]}
  pad_len_m: 0.030              # bottom 3 cm of each jaw's inner face is the pad
  pad_mu: 1.0                   # MEASURE: PLA jaws (often taped); Stringman's rubber pads were 1.5
  jaw_closed_deg: -10.0         # SO-101 gripper joint lower limit: the pads touch (lerobot 0 = fully closed)
  jaw_full_open_deg: 100.0      # joint upper limit: the moving jaw horizontal, tips 10 cm out
  jaw_open_deg: 50.0            # closure 0 (MEASURE with the real toys and socks). Tip gap from the meshes
                                # (2026-10-09): 0 deg 16 mm, 30 deg 53, 50 deg 75, 70 deg 83 (widest), 100 deg 18. Opening
                                # lifts the moving tip: 50 deg puts it 19 mm above the fixed jaw's (-85 vs -104 mm)
  swing_length_m: 0.217         # the twin's pendulum with this hand: period 0.935 s (2026-10-09). MEASURE: polecal
  jaw_open_gap_m: 0.0745        # tip gap at jaw_open_deg (the mesh table above): the task centres the object in it
  wrist_range_deg: [-157.2, 162.8]   # wrist_roll joint limits (-2.7438, 2.8412 rad)
  # STS3215. Joint side (damping, armature, friction): the `sts3215` class, a fit adapted from Open Duck Mini (so101
  # README). Its kp 998 / kv 2.73 are NOT what the real gripper does: they saturate at 0.17 deg of error, so in the twin
  # the load banged +-100 % while closing and read 100 % on every hold (hand_feel 2026-10-09). LeRobot's so_follower
  # configure() sets P_Coefficient 16 (default 32) on every servo and, on the gripper, Max_Torque_Limit 500 (50 %),
  # Protection_Current 250, Overload_Torque 25. Refit on the REAL gripper holding an 11 mm pen (armpilot runs
  # 2026-10-07/08, Present_Load / 10 = % of max torque): squeezed 0.020 rad past contact -> 8.8-10.4 %; told fully
  # closed, stopped 0.08 rad short -> 26-28 % held (31-44 % peak while closing); kp ~10 N m/rad fits both (6.8 + friction,
  # 27 %). Slow free close (~0.2 rad/s, feel_grasp 2026-10-07): ~4 % load; damping 0.60 predicts 4.3 %, kept.
  servo: {stall_Nm: 2.94, damping: 0.60, armature: 0.028, frictionloss: 0.052}
  jaw_servo: {kp: 10.0, kv: 0.3, torque_limit_frac: 0.5}     # kv: MEASURE (lerobot D_Coefficient 32, units unknown)
  wrist_servo: {kp: 10.0, kv: 0.3, torque_limit_frac: 1.0}   # MEASURE: same P_Coefficient 16, assumed the jaw's kp
  servo_max_dps: 270.0          # MEASURE: STS3215 12 V datasheet 0.222 s / 60 deg no load. The host's ramp on the
                                # jaw and the wrist (lerobot sends goal positions; the servo moves at its own speed)
  camera_pos: [0.0025, 0.0734, 0.0066]        # `wrist_camera` link origin; its +z (optical axis) points at the jaw tips
  camera_xyaxes: [1, 0, 0, 0, 0.906, -0.424]  # MuJoCo looks along -z: z_cam = -(optical axis); 25 deg off straight down
  camera_fovy_deg: 70.70        # armpilot calib/wrist.json (640x480, 1.54 px rms, 2026-10)
  camera_res: [640, 480]
  grasp_local: [-0.0079, 0.0, -0.098]   # lerobot `gripperframe`: on the fixed jaw's face. An object of width w sits
                                        # w/2 further along +x: OFF the pole axis (the tasks must rotate it by the wrist)
  load_lsb_pct: 0.1             # STS3215 Present_Load: 0.1 % of max torque per count (Feetech memory table, 0-1000)

sim:
  timestep_s: 0.004             # plant; checked against 0.002 (fw-sim) in tests/test_scene.py