init
This commit is contained in:
cheng
@@ -0,0 +1,304 @@
|
||||
import time
|
||||
|
||||
from gripper_control.filters import clamp
|
||||
|
||||
|
||||
class SideTriggerGripController:
|
||||
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):
|
||||
self.reader.start()
|
||||
|
||||
try:
|
||||
self.gripper.connect()
|
||||
open_force = int(clamp(
|
||||
self.config.open_force,
|
||||
self.config.force_min,
|
||||
self.config.force_max,
|
||||
))
|
||||
print("demo02 start: open gripper and wait for side touch")
|
||||
self.gripper.open(open_force, "startup-open")
|
||||
|
||||
self._print_thresholds()
|
||||
print(
|
||||
f"Calibrating baseline for {self.config.baseline_seconds:.2f}s. "
|
||||
"Keep sensors unloaded."
|
||||
)
|
||||
baseline = self.feedback_processor.calibrate_baseline(self.reader)
|
||||
print(
|
||||
f"baseline normal: L={baseline.left_normal:.4f}{self.normal_unit} "
|
||||
f"R={baseline.right_normal:.4f}{self.normal_unit}; "
|
||||
f"shear: L={baseline.left_shear:.4f}{self.shear_unit} "
|
||||
f"R={baseline.right_shear:.4f}{self.shear_unit}"
|
||||
)
|
||||
|
||||
self._run_event_monitor()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nStopping demo02.")
|
||||
finally:
|
||||
self.gripper.final_open_if_needed()
|
||||
self.reader.stop()
|
||||
|
||||
def _print_thresholds(self):
|
||||
print(
|
||||
"Trigger thresholds: "
|
||||
f"normal={self.config.trigger_normal_force:.3f}{self.normal_unit}, "
|
||||
f"shear={self.config.trigger_shear_force:.3f}{self.shear_unit}"
|
||||
)
|
||||
print(
|
||||
"Grip/release thresholds: "
|
||||
f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, "
|
||||
f"requires_both={int(self.config.grip_requires_both)}, "
|
||||
f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, "
|
||||
f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}"
|
||||
)
|
||||
|
||||
def _run_event_monitor(self):
|
||||
config = self.config
|
||||
interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.05
|
||||
|
||||
state = "open_wait"
|
||||
state_start = time.perf_counter()
|
||||
last_force_ramp_time = state_start
|
||||
recover_quiet_since = None
|
||||
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
|
||||
hold_pos = None
|
||||
reference = None
|
||||
|
||||
while True:
|
||||
tick_start = time.perf_counter()
|
||||
now = time.perf_counter()
|
||||
|
||||
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
|
||||
if feedback is None:
|
||||
print(f"state={state} action=waiting-for-sensor-samples")
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
|
||||
trigger = self._side_triggered(feedback)
|
||||
grip_contact = self._grip_contact(feedback)
|
||||
normal_change = 0.0
|
||||
shear_change = 0.0
|
||||
action = "wait"
|
||||
|
||||
if state == "open_wait":
|
||||
if trigger:
|
||||
force_pct = int(clamp(
|
||||
config.close_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.close(force_pct, "side-trigger-close")
|
||||
state = "closing"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
recover_quiet_since = None
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "side-trigger-close"
|
||||
else:
|
||||
action = "open-wait-touch"
|
||||
|
||||
elif state == "closing":
|
||||
if grip_contact:
|
||||
force_pct = int(clamp(
|
||||
config.grip_start_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
hold_pos = self.gripper.hold_current_position(force_pct)
|
||||
state = "gripping"
|
||||
state_start = now
|
||||
last_force_ramp_time = now
|
||||
reference = None
|
||||
action = "object-detected-low-force"
|
||||
elif now - state_start >= config.close_timeout_seconds:
|
||||
force_pct = int(clamp(
|
||||
config.open_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(force_pct, "close-timeout-open")
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "close-timeout-open"
|
||||
else:
|
||||
action = "closing-to-grip"
|
||||
|
||||
elif state == "gripping":
|
||||
if (
|
||||
now - last_force_ramp_time >= config.force_ramp_interval
|
||||
and force_pct < config.hold_force
|
||||
):
|
||||
force_pct = int(clamp(
|
||||
force_pct + config.force_ramp_step,
|
||||
config.force_min,
|
||||
config.hold_force,
|
||||
))
|
||||
self.gripper.set_force(force_pct)
|
||||
last_force_ramp_time = now
|
||||
action = "grip-ramp-up"
|
||||
elif (
|
||||
force_pct >= config.hold_force
|
||||
and now - state_start >= config.grip_settle_seconds
|
||||
):
|
||||
reference = self._make_reference(feedback)
|
||||
state = "hold_check"
|
||||
state_start = now
|
||||
action = "hold-reference-armed"
|
||||
else:
|
||||
action = "grip-hold"
|
||||
|
||||
elif state == "hold_check":
|
||||
normal_change, shear_change = self._release_changes(feedback, reference)
|
||||
should_release = (
|
||||
normal_change >= config.release_normal_change
|
||||
or shear_change >= config.release_shear_change
|
||||
)
|
||||
if should_release:
|
||||
force_pct = int(clamp(
|
||||
config.open_force,
|
||||
config.force_min,
|
||||
config.force_max,
|
||||
))
|
||||
self.gripper.open(force_pct, "force-change-release-open")
|
||||
self.feedback_processor.reset_filters()
|
||||
state = "open_recover"
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "force-change-release-open"
|
||||
else:
|
||||
action = "hold-check"
|
||||
|
||||
elif state == "open_recover":
|
||||
if self._recover_quiet(feedback):
|
||||
if recover_quiet_since is None:
|
||||
recover_quiet_since = now
|
||||
action = "open-recover-quiet"
|
||||
elif (
|
||||
now - recover_quiet_since >= config.recover_stable_seconds
|
||||
and now - state_start >= config.rearm_seconds
|
||||
):
|
||||
state = "open_wait"
|
||||
state_start = now
|
||||
recover_quiet_since = None
|
||||
hold_pos = None
|
||||
reference = None
|
||||
action = "open-ready"
|
||||
else:
|
||||
action = "open-recover-stabilizing"
|
||||
else:
|
||||
recover_quiet_since = None
|
||||
action = "open-recover-wait-force-clear"
|
||||
|
||||
self._log_tick(
|
||||
feedback=feedback,
|
||||
state=state,
|
||||
trigger=trigger,
|
||||
grip_contact=grip_contact,
|
||||
normal_change=normal_change,
|
||||
shear_change=shear_change,
|
||||
hold_pos=hold_pos,
|
||||
force_pct=force_pct,
|
||||
action=action,
|
||||
)
|
||||
|
||||
sleep_time = interval - (time.perf_counter() - tick_start)
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
def _side_triggered(self, feedback, reference=None):
|
||||
if reference is not None:
|
||||
normal_change, shear_change = self._release_changes(feedback, reference)
|
||||
if (
|
||||
normal_change >= self.config.trigger_normal_change
|
||||
or shear_change >= self.config.trigger_shear_change
|
||||
):
|
||||
return True
|
||||
|
||||
return (
|
||||
feedback.max_normal >= self.config.trigger_normal_force
|
||||
or feedback.max_shear >= self.config.trigger_shear_force
|
||||
)
|
||||
|
||||
def _grip_contact(self, feedback):
|
||||
left_contact = feedback.left_normal >= self.config.grip_normal_force
|
||||
right_contact = feedback.right_normal >= self.config.grip_normal_force
|
||||
if self.config.grip_requires_both:
|
||||
return left_contact and right_contact
|
||||
return left_contact or right_contact
|
||||
|
||||
def _recover_quiet(self, feedback):
|
||||
return (
|
||||
feedback.max_normal <= self.config.recover_normal_force
|
||||
and feedback.max_shear <= self.config.recover_shear_force
|
||||
)
|
||||
|
||||
def _make_reference(self, feedback):
|
||||
return {
|
||||
"left_normal": feedback.left_normal,
|
||||
"right_normal": feedback.right_normal,
|
||||
"max_normal": feedback.max_normal,
|
||||
"left_shear": feedback.left_shear,
|
||||
"right_shear": feedback.right_shear,
|
||||
"max_shear": feedback.max_shear,
|
||||
}
|
||||
|
||||
def _release_changes(self, feedback, reference):
|
||||
if reference is None:
|
||||
return 0.0, 0.0
|
||||
normal_change = max(
|
||||
abs(feedback.left_normal - reference["left_normal"]),
|
||||
abs(feedback.right_normal - reference["right_normal"]),
|
||||
abs(feedback.max_normal - reference["max_normal"]),
|
||||
)
|
||||
shear_change = max(
|
||||
abs(feedback.left_shear - reference["left_shear"]),
|
||||
abs(feedback.right_shear - reference["right_shear"]),
|
||||
abs(feedback.max_shear - reference["max_shear"]),
|
||||
)
|
||||
return normal_change, shear_change
|
||||
|
||||
def _release_contact_lost(self, feedback):
|
||||
return (
|
||||
feedback.max_normal <= self.config.release_contact_lost_normal_force
|
||||
and feedback.max_shear <= self.config.release_contact_lost_shear_force
|
||||
)
|
||||
|
||||
def _log_tick(
|
||||
self,
|
||||
feedback,
|
||||
state,
|
||||
trigger,
|
||||
grip_contact,
|
||||
normal_change,
|
||||
shear_change,
|
||||
hold_pos,
|
||||
force_pct,
|
||||
action,
|
||||
):
|
||||
print(
|
||||
f"L={feedback.left_normal:7.3f}{self.normal_unit} "
|
||||
f"R={feedback.right_normal:7.3f}{self.normal_unit} "
|
||||
f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
|
||||
f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
|
||||
f"trigger={int(trigger)} grip_contact={int(grip_contact)} "
|
||||
f"dN={normal_change:7.3f}{self.normal_unit} "
|
||||
f"dShear={shear_change:7.3f}{self.shear_unit} "
|
||||
f"state={state} hold_pos={hold_pos} "
|
||||
f"force_pct={force_pct:3d} action={action}"
|
||||
)
|
||||
Reference in New Issue
Block a user