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