cheng
223 lines
10 KiB
Python
223 lines
10 KiB
Python
import argparse
|
|
import copy
|
|
import importlib.util
|
|
import json
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
|
|
from .calibration import (
|
|
DEFAULT_NORMAL_FORCE_CALIBRATION,
|
|
DEFAULT_SHEAR_FORCE_CALIBRATION,
|
|
)
|
|
from .paths import PACKAGE_DIR
|
|
|
|
|
|
DEFAULT_CONTROL_CONFIG = str(PACKAGE_DIR / "config" / "gripper_timed_cycle_control.py")
|
|
|
|
|
|
def load_control_config(path):
|
|
suffix = Path(path).suffix.lower()
|
|
if suffix == ".py":
|
|
return load_python_config(path)
|
|
return load_json_config(path)
|
|
|
|
|
|
def load_python_config(path):
|
|
try:
|
|
spec = importlib.util.spec_from_file_location("gripper_user_config", path)
|
|
if spec is None or spec.loader is None:
|
|
raise RuntimeError("cannot create import spec")
|
|
module = importlib.util.module_from_spec(spec)
|
|
spec.loader.exec_module(module)
|
|
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 {}
|
|
|
|
config = getattr(module, "CONFIG", None)
|
|
if config is None:
|
|
config = {
|
|
key.lower(): value
|
|
for key, value in vars(module).items()
|
|
if key.isupper()
|
|
}
|
|
if not isinstance(config, dict):
|
|
print(f"[Error] Python config {path} must define CONFIG as a dict; using CLI/default values")
|
|
return {}
|
|
return config
|
|
|
|
|
|
def load_json_config(path):
|
|
try:
|
|
with open(path, "r", encoding="utf-8") as f:
|
|
return 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 {}
|
|
|
|
|
|
def _config_default(config, key, default):
|
|
return config.get(key, config.get(key.replace("_", "-"), default))
|
|
|
|
|
|
@dataclass
|
|
class ControlConfig:
|
|
control_config: str = DEFAULT_CONTROL_CONFIG
|
|
|
|
video1: str = "0"
|
|
video2: str = "1"
|
|
rotation1: str = "./config/rotation_config_0.json"
|
|
rotation2: str = "./config/rotation_config_1.json"
|
|
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 = "COM3"
|
|
slave_id: int = 1
|
|
baudrate: int = 115200
|
|
serial_timeout: float = 0.2
|
|
|
|
open_pos: int = 0
|
|
close_pos: int = 9000
|
|
open_seconds: float = 3.0
|
|
close_seconds: float = 5.0
|
|
cycles: int = 0
|
|
open_at_end: bool = False
|
|
|
|
speed: int = 20
|
|
accel: int = 60
|
|
decel: int = 60
|
|
|
|
initial_force: int = 10
|
|
grip_start_force: int = 10
|
|
force_min: int = 10
|
|
force_max: int = 60
|
|
control_hz: float = 10.0
|
|
|
|
both_contact_force: float = 0.05
|
|
object_force: float = 0.08
|
|
object_requires_both_contact: bool = True
|
|
empty_release_seconds: float = 1.0
|
|
close_command_interval: float = 0.5
|
|
hold_fallback_pos: int | None = None
|
|
|
|
force_ramp_step: int = 1
|
|
force_ramp_interval: float = 0.3
|
|
shear_hold_force: float = 0.05
|
|
shear_stable_delta: float = 0.02
|
|
shear_stable_seconds: float = 0.5
|
|
human_hold_seconds: float = 1.0
|
|
release_shear_change: float = 0.5
|
|
|
|
emergency_touch_force: float = 2.5
|
|
filter_alpha: float = 0.25
|
|
shear_filter_alpha: float = 0.35
|
|
baseline_seconds: float = 1.0
|
|
|
|
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)
|
|
)
|
|
|
|
trigger_force_update: bool = False
|
|
enable_human_release: bool = True
|
|
enable_emergency_open: bool = False
|
|
dry_run: bool = False
|
|
|
|
@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", cls.rotation1))
|
|
add("--rotation2", "-r2", type=str, default=get("rotation2", cls.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("--open-seconds", type=float, default=get("open_seconds", cls.open_seconds))
|
|
add("--close-seconds", type=float, default=get("close_seconds", cls.close_seconds))
|
|
add("--cycles", type=int, default=get("cycles", cls.cycles))
|
|
add("--open-at-end", action="store_true", default=get("open_at_end", cls.open_at_end))
|
|
|
|
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("--initial-force", type=int, default=get("initial_force", cls.initial_force))
|
|
add("--grip-start-force", type=int, default=get("grip_start_force", cls.grip_start_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("--both-contact-force", type=float, default=get("both_contact_force", cls.both_contact_force))
|
|
add("--object-force", "--clamp-force", dest="object_force", type=float, default=get("object_force", cls.object_force))
|
|
add("--object-requires-both-contact", dest="object_requires_both_contact", action="store_true", default=get("object_requires_both_contact", cls.object_requires_both_contact))
|
|
add("--allow-single-side-object", dest="object_requires_both_contact", action="store_false")
|
|
add("--empty-release-seconds", type=float, default=get("empty_release_seconds", cls.empty_release_seconds))
|
|
add("--close-command-interval", type=float, default=get("close_command_interval", cls.close_command_interval))
|
|
add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
|
|
|
|
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("--shear-hold-force", type=float, default=get("shear_hold_force", cls.shear_hold_force))
|
|
add("--shear-stable-delta", type=float, default=get("shear_stable_delta", cls.shear_stable_delta))
|
|
add("--shear-stable-seconds", type=float, default=get("shear_stable_seconds", cls.shear_stable_seconds))
|
|
add("--human-hold-seconds", type=float, default=get("human_hold_seconds", cls.human_hold_seconds))
|
|
add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
|
|
add("--emergency-touch-force", type=float, default=get("emergency_touch_force", cls.emergency_touch_force))
|
|
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("--baseline-seconds", type=float, default=get("baseline_seconds", cls.baseline_seconds))
|
|
|
|
add("--target-touch-force", type=float, default=1500.0, help=argparse.SUPPRESS)
|
|
add("--deadband", type=float, default=80.0, help=argparse.SUPPRESS)
|
|
add("--step-up", type=int, default=1, help=argparse.SUPPRESS)
|
|
add("--step-down", type=int, default=2, help=argparse.SUPPRESS)
|
|
add("--shear-spike-threshold", type=float, default=0.05, help=argparse.SUPPRESS)
|
|
|
|
add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))
|
|
add("--enable-human-release", dest="enable_human_release", action="store_true", default=get("enable_human_release", cls.enable_human_release))
|
|
add("--disable-human-release", dest="enable_human_release", action="store_false")
|
|
add("--enable-emergency-open", action="store_true", default=get("enable_emergency_open", cls.enable_emergency_open))
|
|
add("--dry-run", action="store_true", default=get("dry_run", cls.dry_run))
|
|
|
|
args = parser.parse_args(argv)
|
|
values = vars(args)
|
|
for legacy_key in ("target_touch_force", "deadband", "step_up", "step_down", "shear_spike_threshold"):
|
|
values.pop(legacy_key, None)
|
|
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)
|