Robotics: simulate, sample, log — then practice teleop

Download .py · mujoco_pendulum_energy.py

#!/usr/bin/env python3
"""BSV recipe: swing a simple pendulum in MuJoCo and log energy drift.

Research / education / simulation only. Not a safety certification or real-hardware procedure.

Input : duration seconds (default 5), timestep hint via model (0.002).
Output: pendulum_energy.csv with time, angle, omega, kinetic, potential, total.
Original BSV code (model), MIT. Library: MuJoCo (Apache-2.0).
"""
import csv, sys
import mujoco
import numpy as np

XML = """
<mujoco model="bsv_pendulum">
  <option timestep="0.002" gravity="0 0 -9.81"/>
  <worldbody>
    <body name="pole" pos="0 0 0">
      <joint name="hinge" type="hinge" axis="0 1 0" damping="0.05"/>
      <geom type="capsule" fromto="0 0 0 0 0 -0.3" size="0.01" mass="0.2"/>
      <site name="tip" pos="0 0 -0.3" size="0.005"/>
    </body>
  </worldbody>
</mujoco>
"""

def main(duration="5"):
    model = mujoco.MjModel.from_xml_string(XML)
    data = mujoco.MjData(model)
    data.qpos[0] = np.deg2rad(120.0)  # start near inverted side, will fall
    data.qvel[0] = 0.0
    mujoco.mj_forward(model, data)
    tip = model.site("tip").id
    g = abs(model.opt.gravity[2])
    # tip mass approx from geom mass
    m = 0.2
    L = 0.3
    rows = []
    steps = int(float(duration) / model.opt.timestep)
    for i in range(steps):
        mujoco.mj_step(model, data)
        th = float(data.qpos[0])
        w = float(data.qvel[0])
        # potential relative to lowest point (theta=±pi... use height of tip)
        z = float(data.site_xpos[tip][2])
        pe = m * g * (z + L)  # 0 at bottom z=-L
        ke = 0.5 * m * (L * w) ** 2
        rows.append([i * model.opt.timestep, th, w, ke, pe, ke + pe])
    with open("pendulum_energy.csv", "w", newline="", encoding="utf-8") as f:
        wri = csv.writer(f)
        wri.writerow(["t_s", "theta_rad", "omega_rad_s", "ke_j", "pe_j", "total_j"])
        wri.writerows(rows)
    totals = [r[-1] for r in rows]
    drift = max(totals) - min(totals)
    print(f"wrote {len(rows)} rows -> pendulum_energy.csv; energy range drift≈{drift:.6f} J (damping present)")

if __name__ == "__main__":
    main(*sys.argv[1:])

Run on 2026-10-07 02:07 JST · Python 3.13.5, mujoco 3.15.0, numpy 2.5.3

wrote 2500 rows -> pendulum_energy.csv; energy range drift≈0.886669 J (damping present)

Download .py · planar_ik_workspace.py

#!/usr/bin/env python3
"""BSV recipe: sample a planar 2-link arm workspace and solve elbow-down IK.

Research / education / simulation only.

Input : L1, L2 in meters (default 0.12,0.10), grid half-extent mm (default 200), step mm (default 10).
Output: ik_workspace.csv with x_mm,y_mm,reachable,q1_rad,q2_rad,err_mm.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

def ik(x, y, L1, L2):
    r2 = x * x + y * y
    c2 = (r2 - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if abs(c2) > 1.0:
        return None
    q2 = float(np.arccos(np.clip(c2, -1, 1)))
    q1 = float(np.arctan2(y, x) - np.arctan2(L2 * np.sin(q2), L1 + L2 * np.cos(q2)))
    return q1, q2

def fk(q1, q2, L1, L2):
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return float(x), float(y)

def main(L1="0.12", L2="0.10", half_mm="200", step_mm="10"):
    L1, L2 = float(L1), float(L2)
    half, step = float(half_mm) / 1000.0, float(step_mm) / 1000.0
    xs = np.arange(-half, half + 1e-9, step)
    ys = np.arange(-half, half + 1e-9, step)
    rows, ok = [], 0
    for x in xs:
        for y in ys:
            sol = ik(x, y, L1, L2)
            if sol is None:
                rows.append([x * 1000, y * 1000, 0, "", "", ""])
                continue
            q1, q2 = sol
            xr, yr = fk(q1, q2, L1, L2)
            err = ((xr - x) ** 2 + (yr - y) ** 2) ** 0.5 * 1000
            rows.append([x * 1000, y * 1000, 1, q1, q2, err])
            ok += 1
    with open("ik_workspace.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f)
        w.writerow(["x_mm", "y_mm", "reachable", "q1_rad", "q2_rad", "fk_err_mm"])
        w.writerows(rows)
    print(f"grid {len(xs)}×{len(ys)} = {len(rows)} cells, reachable {ok} -> ik_workspace.csv")

if __name__ == "__main__":
    main(*sys.argv[1:])

Run on 2026-10-07 02:07 JST · Python 3.13.5, numpy 2.5.3

grid 41×41 = 1681 cells, reachable 1450 -> ik_workspace.csv

Download .py · joint_traj_resample.py

#!/usr/bin/env python3
"""BSV recipe: resample a joint-space trajectory CSV onto a uniform time grid.

Research / education / simulation only. Synthetic default file is labeled example-row.

Input : path to CSV with columns t_s,q1,q2,... (default: write labeled sample), dt seconds (default 0.02).
Output: traj_resampled.csv
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

SAMPLE = """t_s,q1,q2,note
0.00,0.0,0.0,example-row
0.10,0.2,0.1,example-row
0.25,0.5,0.4,example-row
0.40,0.7,0.6,example-row
0.55,0.6,0.5,example-row
"""

def main(path=None, dt="0.02"):
    if not path:
        path = "traj_sample_labeled.csv"
        open(path, "w", encoding="utf-8").write(SAMPLE)
        print("wrote labeled synthetic sample:", path)
    rows = list(csv.DictReader(open(path, encoding="utf-8", newline="")))
    if len(rows) < 2:
        raise SystemExit("need at least 2 rows")
    cols = [c for c in rows[0].keys() if c != "note"]
    if "t_s" not in cols:
        raise SystemExit("CSV must include t_s")
    joint_cols = [c for c in cols if c != "t_s"]
    t = np.array([float(r["t_s"]) for r in rows], dtype=float)
    Q = np.array([[float(r[c]) for c in joint_cols] for r in rows], dtype=float)
    dt = float(dt)
    t_new = np.arange(t[0], t[-1] + 1e-12, dt)
    Qn = np.vstack([np.interp(t_new, t, Q[:, j]) for j in range(Q.shape[1])]).T
    with open("traj_resampled.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f)
        w.writerow(["t_s", *joint_cols])
        for i, ti in enumerate(t_new):
            w.writerow([f"{ti:.6f}", *[f"{v:.8f}" for v in Qn[i]]])
    print(f"resampled {len(rows)} -> {len(t_new)} rows at dt={dt}s -> traj_resampled.csv")

if __name__ == "__main__":
    main(*sys.argv[1:])

Run on 2026-10-07 02:07 JST · Python 3.13.5, numpy 2.5.3

wrote labeled synthetic sample: traj_sample_labeled.csv
resampled 5 -> 28 rows at dt=0.02s -> traj_resampled.csv

Download .py · dh_fk_chain.py

#!/usr/bin/env python3
"""BSV recipe: forward kinematics for a short DH (modified) chain → pose table.

Research / education / simulation only.

Input : comma-separated joint angles in radians (default 0.2,0.4,-0.3) for a fixed 3-revolute demo chain.
Output: dh_fk.csv with link index, x,y,z (meters) of each frame origin, and final RPY (xyz, rad).
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

# Modified DH: alpha_{i-1}, a_{i-1}, d_i, theta_i  (Craig style)
# Demo 3R anthropomorphic-ish lengths (meters)
DH = [
    # alpha, a, d, theta_offset
    (0.0, 0.0, 0.10, 0.0),
    (-np.pi / 2, 0.0, 0.0, 0.0),
    (0.0, 0.25, 0.0, 0.0),
]

def T_mdh(alpha, a, d, theta):
    ca, sa = np.cos(alpha), np.sin(alpha)
    ct, st = np.cos(theta), np.sin(theta)
    return np.array([
        [ct, -st, 0, a],
        [st * ca, ct * ca, -sa, -sa * d],
        [st * sa, ct * sa, ca, ca * d],
        [0, 0, 0, 1],
    ], dtype=float)

def rpy_xyz(R):
    # XYZ fixed angles
    sy = -R[2, 0]
    cy = np.sqrt(max(0.0, 1 - sy * sy))
    if cy > 1e-8:
        rx = np.arctan2(R[2, 1], R[2, 2])
        rz = np.arctan2(R[1, 0], R[0, 0])
    else:
        rx = np.arctan2(-R[1, 2], R[1, 1])
        rz = 0.0
    ry = np.arctan2(sy, cy)
    return float(rx), float(ry), float(rz)

def main(angles="0.2,0.4,-0.3"):
    q = [float(x) for x in angles.split(",")]
    if len(q) != len(DH):
        raise SystemExit(f"need {len(DH)} angles, got {len(q)}")
    T = np.eye(4)
    rows = [[0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
    for i, ((alpha, a, d, off), qi) in enumerate(zip(DH, q), start=1):
        T = T @ T_mdh(alpha, a, d, qi + off)
        rpy = rpy_xyz(T[:3, :3])
        rows.append([i, T[0, 3], T[1, 3], T[2, 3], *rpy])
    with open("dh_fk.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f)
        w.writerow(["frame", "x_m", "y_m", "z_m", "roll_rad", "pitch_rad", "yaw_rad"])
        w.writerows(rows)
    tip = rows[-1]
    print(f"frames {len(rows)} -> dh_fk.csv; tip xyz=({tip[1]:.4f},{tip[2]:.4f},{tip[3]:.4f})")

if __name__ == "__main__":
    main(*sys.argv[1:])

Run on 2026-10-07 02:07 JST · Python 3.13.5, numpy 2.5.3

frames 4 -> dh_fk.csv; tip xyz=(0.2257,0.0457,0.0026)

Download .py · jacobian_planar_2r.py

#!/usr/bin/env python3
"""BSV recipe: planar 2R geometric Jacobian + manipulability along a joint sweep.

Research / education / simulation only. Not a safety-rated controller.

Input : L1,L2 meters (default 0.12,0.10), q1 start/stop/step rad (default 0,1.57,0.1), q2 fixed (default 0.8).
Output: jacobian_2r.csv with q1,q2,x,y,det_j,manipulability,sigma_min,sigma_max.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

def fk(q1, q2, L1, L2):
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return float(x), float(y)

def J(q1, q2, L1, L2):
    s1, c1 = np.sin(q1), np.cos(q1)
    s12, c12 = np.sin(q1 + q2), np.cos(q1 + q2)
    return np.array([
        [-L1 * s1 - L2 * s12, -L2 * s12],
        [L1 * c1 + L2 * c12, L2 * c12],
    ], dtype=float)

def main(L1="0.12", L2="0.10", q1_start="0", q1_stop="1.57", q1_step="0.1", q2="0.8"):
    L1, L2 = float(L1), float(L2)
    q2 = float(q2)
    qs = np.arange(float(q1_start), float(q1_stop) + 1e-12, float(q1_step))
    rows = []
    for q1 in qs:
        Jac = J(q1, q2, L1, L2)
        x, y = fk(q1, q2, L1, L2)
        det = float(np.linalg.det(Jac))
        # Yoshikawa manipulability sqrt(det(J J^T))
        w = float(np.sqrt(max(0.0, np.linalg.det(Jac @ Jac.T))))
        s = np.linalg.svd(Jac, compute_uv=False)
        rows.append({
            "q1_rad": q1, "q2_rad": q2, "x_m": x, "y_m": y,
            "det_j": det, "manipulability": w,
            "sigma_min": float(s.min()), "sigma_max": float(s.max()),
        })
    with open("jacobian_2r.csv", "w", newline="", encoding="utf-8") as f:
        wri = csv.DictWriter(f, fieldnames=list(rows[0].keys()))
        wri.writeheader(); wri.writerows(rows)
    print(f"wrote {len(rows)} Jacobian samples -> jacobian_2r.csv (min manip={min(r['manipulability'] for r in rows):.4f})")

if __name__ == "__main__":
    main(*sys.argv[1:7])

Run on 2026-10-07 02:27 JST · Python 3.13.5, numpy 2.5.3

wrote 16 Jacobian samples -> jacobian_2r.csv (min manip=0.0086)

Download .py · cubic_joint_path.py

#!/usr/bin/env python3
"""BSV recipe: cubic polynomial joint path between two waypoints (zero end velocities).

Research / education / simulation only. Not a real-time motion planner.

Input : q0,q1 radians (default 0.0,1.2), duration s (default 1.0), dt s (default 0.02).
Output: cubic_path.csv with t_s,q,qd,qdd.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

def main(q0="0.0", q1="1.2", T="1.0", dt="0.02"):
    q0, q1, T, dt = float(q0), float(q1), float(T), float(dt)
    if T <= 0 or dt <= 0:
        raise SystemExit("T and dt must be positive")
    # q(t)=a0+a1 t+a2 t^2+a3 t^3 with q(0)=q0,q(T)=q1,qd(0)=qd(T)=0
    a0, a1 = q0, 0.0
    a2 = 3 * (q1 - q0) / (T * T)
    a3 = -2 * (q1 - q0) / (T * T * T)
    ts = np.arange(0.0, T + 1e-12, dt)
    rows = []
    for t in ts:
        q = a0 + a1 * t + a2 * t * t + a3 * t * t * t
        qd = a1 + 2 * a2 * t + 3 * a3 * t * t
        qdd = 2 * a2 + 6 * a3 * t
        rows.append({"t_s": float(t), "q": float(q), "qd": float(qd), "qdd": float(qdd)})
    with open("cubic_path.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.DictWriter(f, fieldnames=["t_s", "q", "qd", "qdd"])
        w.writeheader(); w.writerows(rows)
    print(f"wrote {len(rows)} cubic samples q0={q0}→q1={q1} T={T}s -> cubic_path.csv")

if __name__ == "__main__":
    main(*sys.argv[1:5])

Run on 2026-10-07 02:27 JST · Python 3.13.5, numpy 2.5.3

wrote 51 cubic samples q0=0.0→q1=1.2 T=1.0s -> cubic_path.csv

Download .py · se3_relative_pose.py

#!/usr/bin/env python3
"""BSV recipe: relative SE(3) pose T_A^{-1} T_B from two XYZ+RPY poses.

Research / education / simulation only. Degrees for RPY input; meters for translation.

Input : pose A as x,y,z,roll,pitch,yaw then pose B (defaults: A origin, B small offset).
Output: se3_relative.csv with relative xyz and rpy_xyz (rad), plus 4x4 printed summary.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

def rpy_to_R(roll, pitch, yaw):
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
    Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
    Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
    return Rz @ Ry @ Rx

def R_to_rpy(R):
    pitch = np.arcsin(np.clip(-R[2, 0], -1, 1))
    if abs(np.cos(pitch)) < 1e-8:
        roll = 0.0
        yaw = np.arctan2(-R[0, 1], R[1, 1])
    else:
        roll = np.arctan2(R[2, 1], R[2, 2])
        yaw = np.arctan2(R[1, 0], R[0, 0])
    return float(roll), float(pitch), float(yaw)

def pose_to_T(x, y, z, roll, pitch, yaw):
    T = np.eye(4)
    T[:3, :3] = rpy_to_R(roll, pitch, yaw)
    T[:3, 3] = [x, y, z]
    return T

def main(ax="0", ay="0", az="0", ar="0", ap="0", aw="0",
         bx="0.1", by="0.05", bz="0.02", br="0.2", bp="0.1", bw="-0.3"):
    nums = [float(v) for v in (ax, ay, az, ar, ap, aw, bx, by, bz, br, bp, bw)]
    Ta = pose_to_T(*nums[:6])
    Tb = pose_to_T(*nums[6:])
    Trel = np.linalg.inv(Ta) @ Tb
    x, y, z = Trel[:3, 3]
    roll, pitch, yaw = R_to_rpy(Trel[:3, :3])
    with open("se3_relative.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f)
        w.writerow(["x_m", "y_m", "z_m", "roll_rad", "pitch_rad", "yaw_rad"])
        w.writerow([x, y, z, roll, pitch, yaw])
    print(f"T_A^{-1} T_B xyz=({x:.4f},{y:.4f},{z:.4f}) rpy=({roll:.4f},{pitch:.4f},{yaw:.4f}) -> se3_relative.csv")

if __name__ == "__main__":
    main(*sys.argv[1:13])

Run on 2026-10-07 02:27 JST · Python 3.13.5, numpy 2.5.3

T_A^-1 T_B xyz=(0.1000,0.0500,0.0200) rpy=(0.2000,0.1000,-0.3000) -> se3_relative.csv

Download .py · rrt_2d_grid.py

#!/usr/bin/env python3
"""BSV recipe: tiny 2D grid RRT from start to goal around a rectangular obstacle.

Research / education / simulation only. Toy planner — not for real robots or safety cases.

Input : optional seed (default 7), max iterations (default 800).
Output: rrt_path.csv (path waypoints) and rrt_nodes.csv (tree nodes), plus printed path length.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, sys
import numpy as np

# World: [0,1]x[0,1], obstacle axis-aligned box
OBS = (0.35, 0.25, 0.65, 0.75)  # xmin,ymin,xmax,ymax
START = np.array([0.1, 0.1])
GOAL = np.array([0.9, 0.9])
STEP = 0.05
GOAL_BIAS = 0.1
GOAL_TOL = 0.06

def collides(p):
    x, y = p
    return OBS[0] <= x <= OBS[2] and OBS[1] <= y <= OBS[3]

def segment_clear(a, b, n=12):
    for t in np.linspace(0, 1, n):
        if collides(a * (1 - t) + b * t):
            return False
    return True

def main(seed="7", max_iter="800"):
    rng = np.random.default_rng(int(seed))
    max_iter = int(max_iter)
    nodes = [START.copy()]
    parent = [-1]
    found = None
    for _ in range(max_iter):
        sample = GOAL.copy() if rng.random() < GOAL_BIAS else rng.random(2)
        dists = np.linalg.norm(np.stack(nodes) - sample, axis=1)
        i = int(np.argmin(dists))
        direction = sample - nodes[i]
        nrm = np.linalg.norm(direction)
        if nrm < 1e-12:
            continue
        new = nodes[i] + direction / nrm * min(STEP, nrm)
        if new[0] < 0 or new[0] > 1 or new[1] < 0 or new[1] > 1:
            continue
        if not segment_clear(nodes[i], new):
            continue
        parent.append(i)
        nodes.append(new)
        if np.linalg.norm(new - GOAL) < GOAL_TOL and segment_clear(new, GOAL):
            parent.append(len(nodes) - 1)
            nodes.append(GOAL.copy())
            found = len(nodes) - 1
            break
    with open("rrt_nodes.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f); w.writerow(["id", "x", "y", "parent"])
        for i, (p, par) in enumerate(zip(nodes, parent)):
            w.writerow([i, p[0], p[1], par])
    path = []
    if found is not None:
        i = found
        while i >= 0:
            path.append(nodes[i]); i = parent[i]
        path.reverse()
    with open("rrt_path.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f); w.writerow(["x", "y"])
        for p in path:
            w.writerow([p[0], p[1]])
    plen = float(sum(np.linalg.norm(path[i + 1] - path[i]) for i in range(len(path) - 1))) if path else float("nan")
    print(f"RRT nodes={len(nodes)} path_pts={len(path)} length={plen:.3f} -> rrt_path.csv, rrt_nodes.csv")

if __name__ == "__main__":
    main(*sys.argv[1:3])

Run on 2026-10-07 02:27 JST · Python 3.13.5, numpy 2.5.3

RRT nodes=91 path_pts=33 length=1.604 -> rrt_path.csv, rrt_nodes.csv

Download .py · pid_step_response.py

#!/usr/bin/env python3
"""BSV recipe: discrete PID step response on a 1st-order plant.

Research / education / simulation only — not a real controller or safety case.
Input : optional Kp,Ki,Kd,plant_tau (defaults 1.2 0.4 0.05 0.8), steps (default 80).
Output: pid_step.csv with t, r, y, u.
Original BSV code, MIT. Dependency: none (stdlib).
"""
import csv, sys

def main(kp="1.2", ki="0.4", kd="0.05", tau="0.8", steps="80"):
    Kp, Ki, Kd, tau, N = float(kp), float(ki), float(kd), float(tau), int(steps)
    dt = 0.05
    y = 0.0; integ = 0.0; prev_e = 0.0; r = 1.0
    rows = []
    a = dt / (tau + dt)  # plant y += a*(u-y)
    for k in range(N):
        e = r - y
        integ += e * dt
        deriv = (e - prev_e) / dt
        u = Kp * e + Ki * integ + Kd * deriv
        y = y + a * (u - y)
        prev_e = e
        rows.append({"t": round(k * dt, 4), "r": r, "y": y, "u": u, "e": e})
    with open("pid_step.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.DictWriter(f, fieldnames=["t", "r", "y", "u", "e"])
        w.writeheader(); w.writerows(rows)
    print(f"wrote {len(rows)} PID samples final_y={y:.4f} -> pid_step.csv")

if __name__ == "__main__":
    main(*sys.argv[1:6])

Run on 2026-10-07 03:12 JST · Python 3.13.5

wrote 80 PID samples final_y=0.7967 -> pid_step.csv

Download .py · manip_ellipse_2r.py

#!/usr/bin/env python3
"""BSV recipe: sample planar 2R manipulability (|det J|) across a joint grid.

Research / education / simulation only.
Input : L1,L2 (default 0.12 0.10), grid n (default 24).
Output: manip_ellipse.csv with q1,q2,detJ,manip.
Original BSV code, MIT. Dependency: math (stdlib).
"""
import csv, math, sys

def main(L1="0.12", L2="0.10", n="24"):
    L1, L2, n = float(L1), float(L2), int(n)
    rows = []
    for i in range(n):
        for j in range(n):
            q1 = -math.pi + 2 * math.pi * i / max(n - 1, 1)
            q2 = -math.pi + 2 * math.pi * j / max(n - 1, 1)
            s1, c1 = math.sin(q1), math.cos(q1)
            s12, c12 = math.sin(q1 + q2), math.cos(q1 + q2)
            # J = [[-L1 s1 - L2 s12, -L2 s12],[L1 c1 + L2 c12, L2 c12]]
            det = (-L1 * s1 - L2 * s12) * (L2 * c12) - (-L2 * s12) * (L1 * c1 + L2 * c12)
            rows.append({"q1": q1, "q2": q2, "detJ": det, "manip": abs(det)})
    with open("manip_ellipse.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.DictWriter(f, fieldnames=["q1", "q2", "detJ", "manip"])
        w.writeheader(); w.writerows(rows)
    best = max(rows, key=lambda r: r["manip"])
    print(f"wrote {len(rows)} configs max|detJ|={best['manip']:.5f} at q=({best['q1']:.3f},{best['q2']:.3f}) -> manip_ellipse.csv")

if __name__ == "__main__":
    main(*sys.argv[1:4])

Run on 2026-10-07 03:12 JST · Python 3.13.5

wrote 576 configs max|detJ|=0.01197 at q=(-1.229,1.503) -> manip_ellipse.csv

Download .py · trapezoid_velocity_profile.py

#!/usr/bin/env python3
"""BSV recipe: trapezoidal velocity profile for a scalar joint move.

Research / education / simulation only — not a motion controller.
Input : distance, vmax, amax (defaults 1.0 0.5 1.0), dt (default 0.01).
Output: trap_profile.csv with t, s, v, a.
Original BSV code, MIT. Dependency: none.
"""
import csv, math, sys

def main(distance="1.0", vmax="0.5", amax="1.0", dt="0.01"):
    D, vmax, amax, dt = abs(float(distance)), abs(float(vmax)), abs(float(amax)), float(dt)
    # time to vmax
    t_acc = vmax / amax
    d_acc = 0.5 * amax * t_acc ** 2
    if 2 * d_acc >= D:  # triangle
        t_acc = math.sqrt(D / amax)
        t_flat = 0.0
        vmax = amax * t_acc
    else:
        t_flat = (D - 2 * d_acc) / vmax
    t_total = 2 * t_acc + t_flat
    rows = []
    t = 0.0
    while t <= t_total + 1e-12:
        if t < t_acc:
            a = amax; v = amax * t; s = 0.5 * amax * t ** 2
        elif t < t_acc + t_flat:
            a = 0.0; v = vmax; s = d_acc + vmax * (t - t_acc)
        else:
            tau = t - (t_acc + t_flat)
            a = -amax; v = vmax - amax * tau; s = D - 0.5 * amax * (t_acc - tau) ** 2
            # cleaner: s = d_acc + vmax*t_flat + vmax*tau - 0.5*amax*tau**2
            s = d_acc + vmax * t_flat + vmax * tau - 0.5 * amax * tau ** 2
        rows.append({"t": round(t, 5), "s": s, "v": v, "a": a})
        t += dt
    with open("trap_profile.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.DictWriter(f, fieldnames=["t", "s", "v", "a"])
        w.writeheader(); w.writerows(rows)
    print(f"wrote {len(rows)} samples t_total={t_total:.4f}s vmax_used={vmax:.4f} -> trap_profile.csv")

if __name__ == "__main__":
    main(*sys.argv[1:5])

Run on 2026-10-07 03:12 JST · Python 3.13.5

wrote 251 samples t_total=2.5000s vmax_used=0.5000 -> trap_profile.csv

Download .py · umeyama_align_2d.py

#!/usr/bin/env python3
"""BSV recipe: Umeyama 2D similarity align of two point sets (toy).

Research / education only.
Input : optional noise sigma (default 0.02), seed (default 3), n points (default 12).
Output: umeyama_2d.csv with estimated scale, rotation_deg, tx, ty, rmse.
Original BSV code, MIT. Dependency: numpy.
"""
import csv, math, sys
import numpy as np

def umeyama(X, Y):
    # X,Y: (n,2) corresponding points; estimate Y ≈ s R X + t
    mu_x = X.mean(axis=0); mu_y = Y.mean(axis=0)
    Xc = X - mu_x; Yc = Y - mu_y
    var_x = (Xc ** 2).sum() / len(X)
    cov = (Yc.T @ Xc) / len(X)
    U, S, Vt = np.linalg.svd(cov)
    R = U @ Vt
    if np.linalg.det(R) < 0:
        U[:, -1] *= -1
        R = U @ Vt
    s = (S.sum() / var_x) if var_x > 1e-12 else 1.0
    t = mu_y - s * R @ mu_x
    aligned = (s * (R @ X.T)).T + t
    rmse = float(np.sqrt(((aligned - Y) ** 2).sum() / len(X)))
    ang = math.degrees(math.atan2(R[1, 0], R[0, 0]))
    return s, ang, float(t[0]), float(t[1]), rmse

def main(sigma="0.02", seed="3", n="12"):
    sigma, seed, n = float(sigma), int(seed), int(n)
    rng = np.random.default_rng(seed)
    X = rng.normal(size=(n, 2))
    s_true, th = 1.3, math.radians(35)
    R = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]])
    t_true = np.array([0.4, -0.2])
    Y = (s_true * (R @ X.T)).T + t_true + rng.normal(scale=sigma, size=X.shape)
    s, ang, tx, ty, rmse = umeyama(X, Y)
    with open("umeyama_2d.csv", "w", newline="", encoding="utf-8") as f:
        w = csv.DictWriter(f, fieldnames=["scale", "rotation_deg", "tx", "ty", "rmse", "scale_true", "rotation_true_deg"])
        w.writeheader()
        w.writerow({"scale": s, "rotation_deg": ang, "tx": tx, "ty": ty, "rmse": rmse,
                    "scale_true": s_true, "rotation_true_deg": math.degrees(th)})
    print(f"Umeyama s={s:.4f} ang={ang:.2f}deg rmse={rmse:.4f} -> umeyama_2d.csv")

if __name__ == "__main__":
    main(*sys.argv[1:4])

Run on 2026-10-07 03:12 JST · Python 3.13.5, numpy 2.5.3

Umeyama s=1.3012 ang=34.90deg rmse=0.0282 -> umeyama_2d.csv