Pastebin

New Paste Add Image

# =========================
# OFFLINE (SYNTHESIS) PHASE
# =========================
given:
  - abstraction state space X2 (must include enough to predict effect of torques;
    commonly EE position/velocity or joint position/velocity, but keep feasible)
  - finite input set U2 = {u_tau^1, ..., u_tau^K}  # discrete torque primitives
  - spec: reach-avoid
  - sampling time Ts_sym (e.g., 10–50 ms)
  - conservative disturbance bounds capturing:
      * unmodeled dynamics
      * torque tracking / filtering
      * discretization and state estimation error

S2 = SCOTS.build_abstraction(dynamics_model_for_torque_mode, X2, U2, Ts_sym, disturbance_bounds)
C2 = SCOTS.synthesize(S2, spec)
C2_impl = export_controller(C2)  # BDD/LUT query structure  :contentReference[oaicite:5]{index=5}

# ======================
# ONLINE (RUNTIME) PHASE
# ======================
connect to Franka using libfranka
set Ts_servo = 1 ms  # 1 kHz torque callback :contentReference[oaicite:6]{index=6}

last_u_idx = index_of_safe_stop_torque()  # often "damping-only" or zeros
tick = 0
N_hold = Ts_sym / Ts_servo

robot.control(  # torque control loop callback (torques) :contentReference[oaicite:7]{index=7}
  callback(state, period):
    # 1) Measure current robot state
    y = measure_output(state)
      # includes q, dq; can compute EE pose/vel if needed

# 2) Every N_hold ticks: query SCOTS policy
    if tick % N_hold == 0:
        x2 = quantize_to_grid(y, X2)
        u_idx = C2_impl.lookup(x2)
        if u_idx is None:
            u_idx = index_of_safe_stop_torque()
        last_u_idx = u_idx

# 3) Convert discrete action index to a joint torque vector
    tau_cmd = torque_primitive_to_joint_torques(last_u_idx, state)

# 4) Add stabilizing structure (strongly recommended):
    #    e.g., always add joint damping / soft posture / safety shaping
    tau_cmd = tau_cmd + damping_term(state.dq)
    tau_cmd = clamp(tau_cmd, torque_limits)

# 5) Rate limiting / smoothing to avoid torque discontinuities (practical)
    tau_cmd = rate_limit(tau_cmd, previous_tau_cmd)
    tau_cmd = low_pass_filter(tau_cmd)

previous_tau_cmd = tau_cmd
    tick += 1
    return Torques(tau_cmd)
)

Settings

Title : [Optional]

Paste Folder : [Optional]

Syntax : [Optional]

Expiration : [Optional]

Status : [Optional]

Password : [Optional]

Description: [Optional]

Tags: [Optional]

Encrypt Paste (?)

You are currently not logged in, this means you can not edit or delete anything you paste. Sign Up or Login

Settings

Site Languages