init

gripper/config/__init__.py

@@ -0,0 +1,4 @@
+from .settings import Demo02Config
+
+
+__all__ = ["Demo02Config"]

gripper/config/calibration.py

@@ -0,0 +1,103 @@
+from dataclasses import dataclass
+
+
+DEFAULT_NORMAL_FORCE_CALIBRATION = {
+    "enabled": False,
+    "points": [],
+}
+
+DEFAULT_SHEAR_FORCE_CALIBRATION = {
+    "enabled": False,
+    "points": [],
+}
+
+
+def parse_calibration_point(point):
+    if isinstance(point, dict):
+        raw = point.get("fnormal", point.get("raw", point.get("x")))
+        force_n = point.get("force_n", point.get("n", point.get("y")))
+        return float(raw), float(force_n)
+    if isinstance(point, (list, tuple)) and len(point) >= 2:
+        return float(point[0]), float(point[1])
+    raise ValueError(f"invalid calibration point: {point!r}")
+
+
+@dataclass(frozen=True)
+class ForceConverter:
+    points: tuple
+    unit: str = "N"
+    enabled: bool = True
+    extrapolate: bool = True
+    clamp_output_min: float | None = 0.0
+    input_mode: str = "magnitude"
+    signed: bool = False
+
+    @classmethod
+    def from_config(cls, calibration, config_name):
+        if calibration is None:
+            calibration = {"enabled": False}
+        if not calibration.get("enabled", True):
+            return cls(points=(), unit="raw", enabled=False)
+
+        method = str(calibration.get("method", "piecewise_linear")).lower()
+        if method != "piecewise_linear":
+            raise ValueError(f"unsupported {config_name} method: {method}")
+
+        points = tuple(sorted(
+            parse_calibration_point(point)
+            for point in calibration.get("points", [])
+        ))
+        if len(points) < 2:
+            raise ValueError(f"{config_name}.points must contain at least two points")
+
+        return cls(
+            points=points,
+            enabled=True,
+            extrapolate=bool(calibration.get("extrapolate", True)),
+            clamp_output_min=calibration.get("clamp_output_min", 0.0),
+            input_mode=str(calibration.get("input", "magnitude")).lower(),
+            signed=bool(calibration.get("signed", False)),
+        )
+
+    def convert(self, raw):
+        if not self.enabled:
+            return float(raw)
+
+        raw = float(raw)
+        points = self.points
+        if raw <= points[0][0]:
+            segment = (points[0], points[1])
+            if not self.extrapolate:
+                return self._clamp(points[0][1])
+        elif raw >= points[-1][0]:
+            segment = (points[-2], points[-1])
+            if not self.extrapolate:
+                return self._clamp(points[-1][1])
+        else:
+            segment = None
+            for left, right in zip(points, points[1:]):
+                if left[0] <= raw <= right[0]:
+                    segment = (left, right)
+                    break
+            if segment is None:
+                return self._clamp(points[-1][1])
+
+        (raw0, n0), (raw1, n1) = segment
+        if raw1 == raw0:
+            value = n0
+        else:
+            ratio = (raw - raw0) / (raw1 - raw0)
+            value = n0 + ratio * (n1 - n0)
+        return self._clamp(value)
+
+    def convert_component(self, raw):
+        if not self.signed:
+            return self.convert(raw)
+        raw = float(raw)
+        sign = -1.0 if raw < 0.0 else 1.0
+        return sign * self.convert(abs(raw))
+
+    def _clamp(self, value):
+        if self.clamp_output_min is not None:
+            value = max(float(self.clamp_output_min), value)
+        return value

gripper/config/loader.py

@@ -0,0 +1,21 @@
+import json
+
+
+def load_control_config(path):
+    return load_json_config(path)
+
+
+def load_json_config(path):
+    try:
+        with open(path, "r", encoding="utf-8") as f:
+            config = json.load(f)
+    except FileNotFoundError:
+        print(f"[Info] control config not found: {path}; using CLI/default values")
+        return {}
+    except Exception as exc:
+        print(f"[Error] failed to read control config {path}: {exc}; using CLI/default values")
+        return {}
+    if not isinstance(config, dict):
+        print(f"[Error] control config {path} must be a JSON object; using CLI/default values")
+        return {}
+    return config

gripper/config/rotation.py

@@ -0,0 +1,38 @@
+import copy
+import json
+import os
+import tempfile
+
+
+DEFAULT_ROTATION1 = {
+    "rotation_angle": 90,
+    "flip_x": True,
+    "flip_y": False,
+}
+
+DEFAULT_ROTATION2 = {
+    "rotation_angle": 90,
+    "flip_x": False,
+    "flip_y": False,
+}
+
+
+def normalize_rotation_config(value, default_config):
+    if value is None:
+        return copy.deepcopy(default_config)
+    if isinstance(value, dict):
+        return copy.deepcopy(value)
+    if isinstance(value, str):
+        text = value.strip()
+        if not text or text.lower() == "default":
+            return copy.deepcopy(default_config)
+        if text.startswith("{"):
+            return json.loads(text)
+    return value
+
+
+def write_temp_rotation_config(config, label):
+    path = os.path.join(tempfile.gettempdir(), f"gripper_{label}_rotation.json")
+    with open(path, "w", encoding="utf-8") as f:
+        json.dump(config, f, ensure_ascii=False, indent=2)
+    return path

gripper/config/settings.py

@@ -0,0 +1,211 @@
+import argparse
+import copy
+from dataclasses import dataclass, field
+
+from .calibration import (
+    DEFAULT_NORMAL_FORCE_CALIBRATION,
+    DEFAULT_SHEAR_FORCE_CALIBRATION,
+)
+from .loader import load_control_config
+from ..common.paths import REPO_ROOT
+from .rotation import (
+    DEFAULT_ROTATION1,
+    DEFAULT_ROTATION2,
+    normalize_rotation_config,
+)
+
+
+DEFAULT_CONTROL_CONFIG = str(
+    REPO_ROOT / "config" / "gripper.json"
+)
+
+
+def _config_default(config, key, default):
+    return config.get(key, config.get(key.replace("_", "-"), default))
+
+
+@dataclass
+class Demo02Config:
+    control_config: str = DEFAULT_CONTROL_CONFIG
+
+    video1: str = "0"
+    video2: str = "1"
+    rotation1: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION1))
+    rotation2: dict | str = field(default_factory=lambda: copy.deepcopy(DEFAULT_ROTATION2))
+    sensor_config1: str = "./config/ddjx01.json"
+    sensor_config2: str = "./config/ddjx01.json"
+    sensor_fps: float = 30.0
+    motion_threshold: float = 0.0
+    sensor_cpu: bool = False
+
+    port: str = "COM5"
+    slave_id: int = 1
+    baudrate: int = 115200
+    serial_timeout: float = 0.2
+
+    open_pos: int = 0
+    close_pos: int = 9000
+    speed: int = 25
+    accel: int = 80
+    decel: int = 80
+    release_open_speed: int = 50
+    release_open_accel: int = 120
+    release_open_decel: int = 120
+
+    open_force: int = 10
+    close_force: int = 10
+    grip_start_force: int = 10
+    hold_force: int = 15
+    initial_force: int = 10
+    force_min: int = 10
+    force_max: int = 60
+
+    control_hz: float = 30.0
+    baseline_seconds: float = 1.0
+    filter_alpha: float = 0.55
+    shear_filter_alpha: float = 0.65
+
+    trigger_normal_force: float = 0.04
+    trigger_shear_force: float = 0.04
+    trigger_clear_stable_seconds: float = 0.2
+    open_wait_stable_samples: int = 12
+    open_wait_stable_range_n: float = 0.03
+    open_wait_stable_trend_n: float = 0.015
+    grip_normal_force: float = 0.05
+    grip_requires_both: bool = True
+    close_timeout_seconds: float = 5.0
+    closing_command_interval: float = 0.2
+    force_ramp_step: int = 1
+    force_ramp_interval: float = 0.1
+    grip_settle_seconds: float = 0.1
+    adaptive_grip_enabled: bool = True
+    adaptive_shear_start_force: float = 0.10
+    adaptive_shear_full_force: float = 0.60
+    adaptive_force_min: int = 15
+    adaptive_force_max: int = 45
+    adaptive_force_step: int = 1
+    adaptive_force_interval: float = 0.15
+    release_arm_delay_seconds: float = 0.8
+    release_normal_change: float = 0.03
+    release_shear_change: float = 0.03
+    rearm_seconds: float = 0.5
+    recover_normal_force: float = 0.02
+    recover_shear_force: float = 0.02
+    recover_stable_seconds: float = 0.4
+    open_position_recover_enabled: bool = True
+    open_position_tolerance: int = 100
+    open_position_stable_seconds: float = 0.1
+    open_position_check_interval: float = 0.1
+
+    hold_fallback_pos: int | None = None
+    trigger_force_update: bool = False
+    open_at_end: bool = True
+    dry_run: bool = False
+
+    normal_force_calibration: dict = field(
+        default_factory=lambda: copy.deepcopy(DEFAULT_NORMAL_FORCE_CALIBRATION)
+    )
+    shear_force_calibration: dict = field(
+        default_factory=lambda: copy.deepcopy(DEFAULT_SHEAR_FORCE_CALIBRATION)
+    )
+
+    @classmethod
+    def from_args(cls, argv=None):
+        bootstrap = argparse.ArgumentParser(add_help=False)
+        bootstrap.add_argument("--control-config", default=DEFAULT_CONTROL_CONFIG)
+        bootstrap_args, _ = bootstrap.parse_known_args(argv)
+        config_data = load_control_config(bootstrap_args.control_config)
+
+        parser = argparse.ArgumentParser()
+        add = parser.add_argument
+        get = lambda key, default: _config_default(config_data, key, default)
+
+        add("--control-config", type=str, default=bootstrap_args.control_config)
+        add("--video1", "-v1", type=str, default=get("video1", cls.video1))
+        add("--video2", "-v2", type=str, default=get("video2", cls.video2))
+        add("--rotation1", "-r1", type=str, default=get("rotation1", copy.deepcopy(DEFAULT_ROTATION1)))
+        add("--rotation2", "-r2", type=str, default=get("rotation2", copy.deepcopy(DEFAULT_ROTATION2)))
+        add("--sensor-config1", type=str, default=get("sensor_config1", cls.sensor_config1))
+        add("--sensor-config2", type=str, default=get("sensor_config2", cls.sensor_config2))
+        add("--sensor-fps", type=float, default=get("sensor_fps", cls.sensor_fps))
+        add("--motion-threshold", type=float, default=get("motion_threshold", cls.motion_threshold))
+        add("--sensor-cpu", action="store_true", default=get("sensor_cpu", cls.sensor_cpu))
+
+        add("--port", type=str, default=get("port", cls.port))
+        add("--slave-id", type=int, default=get("slave_id", cls.slave_id))
+        add("--baudrate", type=int, default=get("baudrate", cls.baudrate))
+        add("--serial-timeout", type=float, default=get("serial_timeout", cls.serial_timeout))
+
+        add("--open-pos", type=int, default=get("open_pos", cls.open_pos))
+        add("--close-pos", type=int, default=get("close_pos", cls.close_pos))
+        add("--speed", type=int, default=get("speed", cls.speed))
+        add("--accel", type=int, default=get("accel", cls.accel))
+        add("--decel", type=int, default=get("decel", cls.decel))
+        add("--release-open-speed", type=int, default=get("release_open_speed", cls.release_open_speed))
+        add("--release-open-accel", type=int, default=get("release_open_accel", cls.release_open_accel))
+        add("--release-open-decel", type=int, default=get("release_open_decel", cls.release_open_decel))
+
+        add("--open-force", type=int, default=get("open_force", cls.open_force))
+        add("--close-force", type=int, default=get("close_force", cls.close_force))
+        add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_force))
+        add("--hold-force", type=int, default=get("hold_force", cls.hold_force))
+        add("--force-min", type=int, default=get("force_min", cls.force_min))
+        add("--force-max", type=int, default=get("force_max", cls.force_max))
+
+        add("--control-hz", type=float, default=get("control_hz", cls.control_hz))
+        add("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
+        add("--filter-alpha", type=float, default=get("filter_alpha", cls.filter_alpha))
+        add("--shear-filter-alpha", type=float, default=get("shear_filter_alpha", cls.shear_filter_alpha))
+
+        add("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force))
+        add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force))
+        add("--trigger-clear-stable-seconds", type=float, default=get("trigger_clear_stable_seconds", cls.trigger_clear_stable_seconds))
+        add("--open-wait-stable-samples", type=int, default=get("open_wait_stable_samples", cls.open_wait_stable_samples))
+        add("--open-wait-stable-range-n", type=float, default=get("open_wait_stable_range_n", cls.open_wait_stable_range_n))
+        add("--open-wait-stable-trend-n", type=float, default=get("open_wait_stable_trend_n", cls.open_wait_stable_trend_n))
+        add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force))
+        add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both))
+        add("--allow-single-side-grip", dest="grip_requires_both", action="store_false")
+        add("--close-timeout-seconds", type=float, default=get("close_timeout_seconds", cls.close_timeout_seconds))
+        add("--closing-command-interval", type=float, default=get("closing_command_interval", cls.closing_command_interval))
+        add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
+        add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
+        add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
+        add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled))
+        add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false")
+        add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force))
+        add("--adaptive-shear-full-force", type=float, default=get("adaptive_shear_full_force", cls.adaptive_shear_full_force))
+        add("--adaptive-force-min", type=int, default=get("adaptive_force_min", cls.adaptive_force_min))
+        add("--adaptive-force-max", type=int, default=get("adaptive_force_max", cls.adaptive_force_max))
+        add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step))
+        add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval))
+        add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds))
+        add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
+        add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
+        add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
+        add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
+        add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
+        add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds))
+        add("--open-position-recover-enabled", action="store_true", default=get("open_position_recover_enabled", cls.open_position_recover_enabled))
+        add("--disable-open-position-recover", dest="open_position_recover_enabled", action="store_false")
+        add("--open-position-tolerance", type=int, default=get("open_position_tolerance", cls.open_position_tolerance))
+        add("--open-position-stable-seconds", type=float, default=get("open_position_stable_seconds", cls.open_position_stable_seconds))
+        add("--open-position-check-interval", type=float, default=get("open_position_check_interval", cls.open_position_check_interval))
+
+        add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
+        add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))
+        add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end))
+        add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run))
+
+        args = parser.parse_args(argv)
+        values = vars(args)
+        values["initial_force"] = values["open_force"]
+        values["rotation1"] = normalize_rotation_config(values["rotation1"], DEFAULT_ROTATION1)
+        values["rotation2"] = normalize_rotation_config(values["rotation2"], DEFAULT_ROTATION2)
+        values["normal_force_calibration"] = copy.deepcopy(
+            config_data.get("normal_force_calibration", DEFAULT_NORMAL_FORCE_CALIBRATION)
+        )
+        values["shear_force_calibration"] = copy.deepcopy(
+            config_data.get("shear_force_calibration", DEFAULT_SHEAR_FORCE_CALIBRATION)
+        )
+        return cls(**values)