init
This commit is contained in:
cheng
@@ -0,0 +1,222 @@
|
||||
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)
|
||||
Reference in New Issue
Block a user