cheng
327 lines
13 KiB
Python
327 lines
13 KiB
Python
import time
|
|
|
|
from .filters import clamp
|
|
|
|
|
|
class TimedCycleGripperController:
|
|
def __init__(self, config, reader, gripper, feedback_processor):
|
|
self.config = config
|
|
self.reader = reader
|
|
self.gripper = gripper
|
|
self.feedback_processor = feedback_processor
|
|
self.normal_unit = feedback_processor.normal_converter.unit
|
|
self.shear_unit = feedback_processor.shear_converter.unit
|
|
|
|
def run(self):
|
|
config = self.config
|
|
self.reader.start()
|
|
|
|
try:
|
|
self._print_thresholds()
|
|
print(
|
|
f"Calibrating baseline for {config.baseline_seconds:.2f}s. "
|
|
"Keep sensors unloaded."
|
|
)
|
|
baseline = self.feedback_processor.calibrate_baseline(self.reader)
|
|
print(
|
|
f"baseline normal: left={baseline.left_normal:.4f}{self.normal_unit}, "
|
|
f"right={baseline.right_normal:.4f}{self.normal_unit}; "
|
|
f"shear: left={baseline.left_shear:.4f}{self.shear_unit}, "
|
|
f"right={baseline.right_shear:.4f}{self.shear_unit}"
|
|
)
|
|
|
|
self.gripper.connect()
|
|
|
|
cycle_index = 1
|
|
while config.cycles <= 0 or cycle_index <= config.cycles:
|
|
open_force = int(clamp(
|
|
config.initial_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
self._run_open_phase(open_force, cycle_index)
|
|
result = self._run_close_phase(cycle_index)
|
|
|
|
if result in ("emergency", "released"):
|
|
time.sleep(config.open_seconds)
|
|
if result == "gripped":
|
|
print("object is gripped; timed open/close loop stopped")
|
|
break
|
|
cycle_index += 1
|
|
|
|
except KeyboardInterrupt:
|
|
print("\nStopping timed cycle control.")
|
|
finally:
|
|
self.gripper.final_open_if_needed()
|
|
self.reader.stop()
|
|
|
|
def _print_thresholds(self):
|
|
config = self.config
|
|
print(
|
|
"Normal force thresholds: "
|
|
f"both_contact={config.both_contact_force:.3f}{self.normal_unit}, "
|
|
f"object={config.object_force:.3f}{self.normal_unit}, "
|
|
f"emergency={config.emergency_touch_force:.3f}{self.normal_unit}, "
|
|
f"requires_both={int(config.object_requires_both_contact)}, "
|
|
f"empty_release={config.empty_release_seconds:.2f}s"
|
|
)
|
|
print(
|
|
"Shear force thresholds: "
|
|
f"hold_min={config.shear_hold_force:.3f}{self.shear_unit}, "
|
|
f"stable_delta={config.shear_stable_delta:.3f}{self.shear_unit}, "
|
|
f"stable_seconds={config.shear_stable_seconds:.2f}s, "
|
|
f"release_change={config.release_shear_change:.3f}{self.shear_unit}"
|
|
)
|
|
|
|
def _run_open_phase(self, force_pct, cycle_index):
|
|
print(f"\ncycle {cycle_index}: open phase")
|
|
self.gripper.open(force_pct, "open")
|
|
|
|
deadline = time.perf_counter() + self.config.open_seconds
|
|
while time.perf_counter() < deadline:
|
|
self.reader.update()
|
|
time.sleep(0.02)
|
|
|
|
def _run_close_phase(self, cycle_index):
|
|
config = self.config
|
|
print(f"cycle {cycle_index}: close phase")
|
|
self.feedback_processor.reset_filters()
|
|
|
|
force_pct = int(clamp(config.initial_force, config.force_min, config.force_max))
|
|
self.gripper.close(force_pct, "close")
|
|
|
|
interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.1
|
|
phase_start = time.perf_counter()
|
|
last_close_command_time = phase_start
|
|
last_force_ramp_time = phase_start
|
|
state = "closing"
|
|
hold_pos = None
|
|
previous_shear = None
|
|
hold_start_time = None
|
|
hold_shear_reference = None
|
|
release_monitor_armed = False
|
|
shear_stable_since = None
|
|
empty_since = None
|
|
gripped = False
|
|
deadline = time.perf_counter() + config.close_seconds
|
|
|
|
while True:
|
|
tick_start = time.perf_counter()
|
|
now = time.perf_counter()
|
|
if state == "closing" and now >= deadline:
|
|
print("close phase finished without object; continue timed open/close cycle")
|
|
return "no_object"
|
|
|
|
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
|
|
if feedback is None:
|
|
phase_time = time.perf_counter() - phase_start
|
|
print(f"t={phase_time:5.2f}s action=timed-close waiting for sensor samples")
|
|
time.sleep(0.05)
|
|
continue
|
|
|
|
has_previous_shear = previous_shear is not None
|
|
shear_delta = 0.0 if not has_previous_shear else feedback.max_shear - previous_shear
|
|
shear_abs_delta = abs(shear_delta)
|
|
previous_shear = feedback.max_shear
|
|
|
|
both_contact = (
|
|
feedback.left_normal >= config.both_contact_force
|
|
and feedback.right_normal >= config.both_contact_force
|
|
)
|
|
normal_force_detected = feedback.max_normal >= config.object_force
|
|
contact_confirmed = (
|
|
normal_force_detected
|
|
and (both_contact or not config.object_requires_both_contact)
|
|
)
|
|
object_detected = contact_confirmed
|
|
grip_contact = (
|
|
both_contact
|
|
if config.object_requires_both_contact
|
|
else feedback.max_normal >= config.both_contact_force
|
|
)
|
|
|
|
if state in ("gripping", "hold_check") and not grip_contact:
|
|
if empty_since is None:
|
|
empty_since = now
|
|
elif now - empty_since >= config.empty_release_seconds:
|
|
force_pct = self.gripper.set_force(config.force_min)
|
|
print(
|
|
f"L={feedback.left_normal:8.3f}{self.normal_unit} "
|
|
f"R={feedback.right_normal:8.3f}{self.normal_unit} "
|
|
f"min={feedback.min_normal:8.3f}{self.normal_unit} "
|
|
f"max={feedback.max_normal:8.3f}{self.normal_unit} "
|
|
f"force_pct={force_pct:3d} action=empty-grip-resume-cycle"
|
|
)
|
|
return "no_object"
|
|
else:
|
|
empty_since = None
|
|
|
|
if feedback.max_normal >= config.emergency_touch_force:
|
|
if config.enable_emergency_open:
|
|
force_pct = self.gripper.set_force(config.force_min)
|
|
self.gripper.open(force_pct, "emergency-open")
|
|
print(
|
|
f"L={feedback.left_normal:8.3f}{self.normal_unit} "
|
|
f"R={feedback.right_normal:8.3f}{self.normal_unit} "
|
|
f"max={feedback.max_normal:8.3f}{self.normal_unit} "
|
|
f"force_pct={force_pct:3d} action=emergency-open"
|
|
)
|
|
return "emergency"
|
|
hold_pos = self.gripper.hold_current_position(force_pct)
|
|
state = "hold_check"
|
|
gripped = True
|
|
if hold_start_time is None:
|
|
hold_start_time = now
|
|
action = "overforce-hold"
|
|
|
|
if state == "closing":
|
|
if contact_confirmed:
|
|
state = "gripping"
|
|
gripped = True
|
|
force_pct = int(clamp(
|
|
config.grip_start_force,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
force_pct = self.gripper.set_force(force_pct)
|
|
hold_pos = self.gripper.hold_current_position(force_pct)
|
|
last_force_ramp_time = now
|
|
shear_stable_since = None
|
|
action = "object-detected-low-force"
|
|
print(
|
|
"object detected; timed open/close cycle stopped, "
|
|
f"grip force reset to {force_pct}"
|
|
)
|
|
else:
|
|
action = "closing"
|
|
if (
|
|
config.close_command_interval > 0
|
|
and now - last_close_command_time >= config.close_command_interval
|
|
):
|
|
self.gripper.close(force_pct, "close-continue")
|
|
last_close_command_time = now
|
|
|
|
elif state == "gripping":
|
|
shear_is_high_enough = feedback.max_shear >= config.shear_hold_force
|
|
shear_is_stable = (
|
|
shear_is_high_enough
|
|
and has_previous_shear
|
|
and shear_abs_delta <= config.shear_stable_delta
|
|
)
|
|
|
|
if shear_is_stable:
|
|
if shear_stable_since is None:
|
|
shear_stable_since = now
|
|
else:
|
|
shear_stable_since = None
|
|
|
|
if (
|
|
shear_stable_since is not None
|
|
and now - shear_stable_since >= config.shear_stable_seconds
|
|
):
|
|
state = "hold_check"
|
|
hold_start_time = now
|
|
hold_shear_reference = None
|
|
release_monitor_armed = False
|
|
hold_pos = self.gripper.hold_current_position(force_pct)
|
|
action = "shear-stable-hold"
|
|
elif shear_is_high_enough:
|
|
action = "wait-shear-stable"
|
|
elif (
|
|
now - last_force_ramp_time >= config.force_ramp_interval
|
|
and force_pct < config.force_max
|
|
):
|
|
force_pct = int(clamp(
|
|
force_pct + config.force_ramp_step,
|
|
config.force_min,
|
|
config.force_max,
|
|
))
|
|
force_pct = self.gripper.set_force(force_pct)
|
|
last_force_ramp_time = now
|
|
action = "grip-ramp-up"
|
|
else:
|
|
action = "grip-hold"
|
|
|
|
elif state == "hold_check":
|
|
hold_elapsed = now - hold_start_time
|
|
if not release_monitor_armed and hold_elapsed >= config.human_hold_seconds:
|
|
hold_shear_reference = feedback.max_shear
|
|
release_monitor_armed = True
|
|
action = "hold-check-armed"
|
|
elif release_monitor_armed:
|
|
hold_shear_change = abs(feedback.max_shear - hold_shear_reference)
|
|
if hold_shear_change >= config.release_shear_change:
|
|
if config.enable_human_release:
|
|
force_pct = self.gripper.set_force(config.force_min)
|
|
self.gripper.open(force_pct, "human-release-open")
|
|
print(
|
|
f"L={feedback.left_normal:8.3f}{self.normal_unit} "
|
|
f"R={feedback.right_normal:8.3f}{self.normal_unit} "
|
|
f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
|
|
f"hold_change={hold_shear_change:8.3f}{self.shear_unit} "
|
|
f"force_pct={force_pct:3d} action=human-release-open"
|
|
)
|
|
return "released"
|
|
hold_shear_reference = feedback.max_shear
|
|
action = "hold-check-change-ignored"
|
|
else:
|
|
action = "hold-check"
|
|
else:
|
|
action = "hold-check"
|
|
|
|
self._log_tick(
|
|
phase_start,
|
|
feedback,
|
|
shear_delta,
|
|
both_contact,
|
|
object_detected,
|
|
grip_contact,
|
|
empty_since,
|
|
shear_stable_since,
|
|
state,
|
|
gripped,
|
|
hold_pos,
|
|
force_pct,
|
|
action,
|
|
)
|
|
|
|
sleep_time = interval - (time.perf_counter() - tick_start)
|
|
if sleep_time > 0:
|
|
time.sleep(sleep_time)
|
|
|
|
def _log_tick(
|
|
self,
|
|
phase_start,
|
|
feedback,
|
|
shear_delta,
|
|
both_contact,
|
|
object_detected,
|
|
grip_contact,
|
|
empty_since,
|
|
shear_stable_since,
|
|
state,
|
|
gripped,
|
|
hold_pos,
|
|
force_pct,
|
|
action,
|
|
):
|
|
now = time.perf_counter()
|
|
phase_time = now - phase_start
|
|
print(
|
|
f"t={phase_time:5.2f}s "
|
|
f"L={feedback.left_normal:8.3f}{self.normal_unit} "
|
|
f"R={feedback.right_normal:8.3f}{self.normal_unit} "
|
|
f"min={feedback.min_normal:8.3f}{self.normal_unit} "
|
|
f"max={feedback.max_normal:8.3f}{self.normal_unit} "
|
|
f"diff={feedback.normal_diff:8.3f}{self.normal_unit} "
|
|
f"both={int(both_contact)} object={int(object_detected)} "
|
|
f"grip_contact={int(grip_contact)} "
|
|
f"empty_for={0.0 if empty_since is None else now - empty_since:4.1f}s "
|
|
f"shear={feedback.max_shear:8.3f}{self.shear_unit} "
|
|
f"d_shear={shear_delta:8.3f}{self.shear_unit} "
|
|
f"shear_stable={int(shear_stable_since is not None)} "
|
|
f"state={state} gripped={int(gripped)} hold_pos={hold_pos} "
|
|
f"force_pct={force_pct:3d} action={action}"
|
|
)
|
|
|