cheng
238 lines
9.1 KiB
Python
238 lines
9.1 KiB
Python
"""夹爪02展示配置。
|
|
|
|
展示逻辑：
|
|
1. 程序启动后夹爪默认张开。
|
|
2. 等待任意一侧触觉传感器感受到力。
|
|
3. 一侧触发后，夹爪开始闭合；你把物体移动到中间。
|
|
4. 闭合时检测到物体后，像 01 一样低力保持当前位置并慢慢加力夹住。
|
|
5. 夹住后法向力或切向力变化超过阈值，直接张开到 OPEN_POS。
|
|
6. 张开后保持初始张开状态，等力清空并稳定后才重新等待单侧触发；不会自己定时开合。
|
|
"""
|
|
|
|
|
|
# 左右触觉传感器视频源。
|
|
VIDEO1 = "0"
|
|
VIDEO2 = "1"
|
|
|
|
# 左右触觉传感器方向配置。
|
|
ROTATION1 = "./config/rotation_config_0.json"
|
|
ROTATION2 = "./config/rotation_config_1.json"
|
|
|
|
# 左右触觉传感器 SDK 配置。
|
|
SENSOR_CONFIG1 = "./config/ddjx01.json"
|
|
SENSOR_CONFIG2 = "./config/ddjx01.json"
|
|
|
|
# 触觉处理参数。
|
|
SENSOR_FPS = 30.0
|
|
MOTION_THRESHOLD = 0.0
|
|
SENSOR_CPU = False
|
|
|
|
|
|
# 夹爪串口配置。
|
|
PORT = "COM5"
|
|
SLAVE_ID = 1
|
|
BAUDRATE = 115200
|
|
SERIAL_TIMEOUT = 0.2
|
|
|
|
|
|
# 夹爪位置。
|
|
OPEN_POS = 0
|
|
CLOSE_POS = 9000
|
|
|
|
# 夹爪运动参数。展示时可以速度稍快一点。
|
|
SPEED = 25
|
|
ACCEL = 80
|
|
DECEL = 80
|
|
RELEASE_OPEN_SPEED = 50 # 检测到人取物/力变化后，张开使用的速度。
|
|
RELEASE_OPEN_ACCEL = 120 # release 张开的加速度。
|
|
RELEASE_OPEN_DECEL = 120 # release 张开的减速度。
|
|
|
|
|
|
# 夹爪力度百分比。
|
|
OPEN_FORCE = 15 # 张开时使用的力。
|
|
CLOSE_FORCE = 15 # 单侧触发后闭合时使用的小力。
|
|
GRIP_START_FORCE = 10 # 闭合检测到物体后，先切到这个低力保持。
|
|
HOLD_FORCE = 15 # 基础夹持力；轻物体默认慢慢加到这个力。
|
|
FORCE_MIN = 10
|
|
FORCE_MAX = 80 # 允许自适应加力的最大总上限；重物夹不住时可以适当加大。
|
|
|
|
|
|
# 控制循环频率。越高检测释放越快。
|
|
CONTROL_HZ = 30.0
|
|
|
|
|
|
# 启动时采集空载基线的时间。
|
|
BASELINE_SECONDS = 1.0
|
|
|
|
# 滤波系数，越大响应越快，越小越平滑。
|
|
FILTER_ALPHA = 0.55
|
|
SHEAR_FILTER_ALPHA = 0.65
|
|
|
|
|
|
# 等待触发：任意一侧超过这些阈值，就开始闭合。
|
|
TRIGGER_NORMAL_FORCE = 0.1 # 单侧法向力触发阈值，单位 N。
|
|
TRIGGER_SHEAR_FORCE = 0.1 # 单侧切向力触发阈值，单位 N。
|
|
TRIGGER_CLEAR_STABLE_SECONDS = 0.2 # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。
|
|
|
|
|
|
# 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
|
|
GRIP_NORMAL_FORCE = 0.05 # 双侧接触阈值，单位 N。
|
|
GRIP_REQUIRES_BOTH = True # True 表示必须左右两侧都接触才算夹住。
|
|
CLOSE_TIMEOUT_SECONDS = 5.0 # 触发闭合后这么久还没夹住，就重新张开等待。
|
|
|
|
|
|
# 检测到物体后慢慢加力，类似 01 的 gripping 状态。
|
|
FORCE_RAMP_STEP = 1
|
|
FORCE_RAMP_INTERVAL = 0.1
|
|
GRIP_SETTLE_SECONDS = 0.1 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
|
|
|
|
# 根据切向力估计物体负载，并慢慢补夹紧力。
|
|
# 逻辑：切向力越大，说明物体越重或越容易下滑，目标夹紧力越高。
|
|
ADAPTIVE_GRIP_ENABLED = True # 是否启用切向力自适应加力。
|
|
ADAPTIVE_SHEAR_START_FORCE = 0.10 # 切向力低于这个值时，认为是轻物体，只用 ADAPTIVE_FORCE_MIN。
|
|
ADAPTIVE_SHEAR_FULL_FORCE = 0.60 # 切向力达到这个值时，认为负载较大，目标力加到 ADAPTIVE_FORCE_MAX。
|
|
ADAPTIVE_FORCE_MIN = 15 # 自适应夹持的最小目标力，通常等于 HOLD_FORCE。
|
|
ADAPTIVE_FORCE_MAX = 45 # 自适应夹持的最大目标力；重物夹不住可以加大，但不要超过 FORCE_MAX。
|
|
ADAPTIVE_FORCE_STEP = 1 # 自适应加力每次增加多少 force_pct。
|
|
ADAPTIVE_FORCE_INTERVAL = 0.15 # 自适应加力两次之间的间隔，单位秒；越大加力越柔和。
|
|
RELEASE_ARM_DELAY_SECONDS = 0.8 # 自适应加力停止后，等待这么久再启用“人取物松开”检测。
|
|
|
|
# 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
|
|
RELEASE_NORMAL_CHANGE = 0.2 # 左/右法向力相对夹住参考值的变化阈值，单位 N。
|
|
RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值，单位 N。
|
|
|
|
# 张开到位或恢复条件成立后，至少休息多久才重新进入等待触发。
|
|
# 休息期间即使有物体扫过，也不会触发闭合。
|
|
REARM_SECONDS = 0.5
|
|
|
|
# 松开/超时张开后，必须等传感器读数低于这些阈值并稳定，才允许下一次触发。
|
|
# 这几个值应小于 TRIGGER_NORMAL_FORCE / TRIGGER_SHEAR_FORCE。
|
|
RECOVER_NORMAL_FORCE = 0.02 # 张开恢复时允许重新触发的最大法向残余力，单位 N。
|
|
RECOVER_SHEAR_FORCE = 0.02 # 张开恢复时允许重新触发的最大切向残余力，单位 N。
|
|
RECOVER_STABLE_SECONDS = 0.4 # 力清空后需要连续稳定这么久，才回到 open_wait。
|
|
|
|
# 松开/超时张开后，如果夹爪已经回到 OPEN_POS 初始点位，也允许重新触发。
|
|
# 这样传感器上还有一点残余力时，不会一直卡在 open_recover。
|
|
OPEN_POSITION_RECOVER_ENABLED = True
|
|
OPEN_POSITION_TOLERANCE = 100 # 当前位置和 OPEN_POS 的差值小于它，就认为回到初始点位。
|
|
OPEN_POSITION_STABLE_SECONDS = 0.1 # 回到初始点位后稳定这么久，才回到 open_wait。
|
|
OPEN_POSITION_CHECK_INTERVAL = 0.1 # open_recover 状态下读取夹爪位置的间隔，单位秒。
|
|
|
|
|
|
# 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。
|
|
HOLD_FALLBACK_POS = None
|
|
|
|
|
|
# SDK 行为开关。
|
|
TRIGGER_FORCE_UPDATE = False
|
|
OPEN_AT_END = True
|
|
DRY_RUN = False
|
|
|
|
|
|
# 法向力标定：FNORMAL 原始值 -> N。
|
|
NORMAL_FORCE_CALIBRATION = {
|
|
"enabled": True,
|
|
"method": "piecewise_linear",
|
|
"extrapolate": True,
|
|
"clamp_output_min": 0.0,
|
|
"points": [
|
|
{"fnormal": 1000.0, "force_n": 0.0},
|
|
{"fnormal": 10000.0, "force_n": 0.377},
|
|
{"fnormal": 30000.0, "force_n": 1.377},
|
|
{"fnormal": 62000.0, "force_n": 2.377},
|
|
],
|
|
}
|
|
|
|
|
|
# 切向力标定：FSHEARX / FSHEARY 原始分量 -> N。
|
|
# X/Y 分量分别按同一条曲线标定；控制逻辑再使用标定后分量的合力。
|
|
SHEAR_FORCE_CALIBRATION = {
|
|
"enabled": True,
|
|
"method": "piecewise_linear",
|
|
"input": "components",
|
|
"signed": True,
|
|
"extrapolate": True,
|
|
"clamp_output_min": 0.0,
|
|
"points": [
|
|
{"raw": 1000.0, "force_n": 0.0},
|
|
{"raw": 36000.0, "force_n": 1.19},
|
|
{"raw": 68000.0, "force_n": 2.06},
|
|
],
|
|
}
|
|
|
|
|
|
CONFIG = {
|
|
"video1": VIDEO1,
|
|
"video2": VIDEO2,
|
|
"rotation1": ROTATION1,
|
|
"rotation2": ROTATION2,
|
|
"sensor_config1": SENSOR_CONFIG1,
|
|
"sensor_config2": SENSOR_CONFIG2,
|
|
"sensor_fps": SENSOR_FPS,
|
|
"motion_threshold": MOTION_THRESHOLD,
|
|
"sensor_cpu": SENSOR_CPU,
|
|
|
|
"port": PORT,
|
|
"slave_id": SLAVE_ID,
|
|
"baudrate": BAUDRATE,
|
|
"serial_timeout": SERIAL_TIMEOUT,
|
|
|
|
"open_pos": OPEN_POS,
|
|
"close_pos": CLOSE_POS,
|
|
"speed": SPEED,
|
|
"accel": ACCEL,
|
|
"decel": DECEL,
|
|
"release_open_speed": RELEASE_OPEN_SPEED,
|
|
"release_open_accel": RELEASE_OPEN_ACCEL,
|
|
"release_open_decel": RELEASE_OPEN_DECEL,
|
|
|
|
"open_force": OPEN_FORCE,
|
|
"close_force": CLOSE_FORCE,
|
|
"grip_start_force": GRIP_START_FORCE,
|
|
"hold_force": HOLD_FORCE,
|
|
"initial_force": OPEN_FORCE,
|
|
"force_min": FORCE_MIN,
|
|
"force_max": FORCE_MAX,
|
|
|
|
"control_hz": CONTROL_HZ,
|
|
"baseline_seconds": BASELINE_SECONDS,
|
|
"filter_alpha": FILTER_ALPHA,
|
|
"shear_filter_alpha": SHEAR_FILTER_ALPHA,
|
|
|
|
"trigger_normal_force": TRIGGER_NORMAL_FORCE,
|
|
"trigger_shear_force": TRIGGER_SHEAR_FORCE,
|
|
"trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS,
|
|
"grip_normal_force": GRIP_NORMAL_FORCE,
|
|
"grip_requires_both": GRIP_REQUIRES_BOTH,
|
|
"close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
|
|
"force_ramp_step": FORCE_RAMP_STEP,
|
|
"force_ramp_interval": FORCE_RAMP_INTERVAL,
|
|
"grip_settle_seconds": GRIP_SETTLE_SECONDS,
|
|
"adaptive_grip_enabled": ADAPTIVE_GRIP_ENABLED,
|
|
"adaptive_shear_start_force": ADAPTIVE_SHEAR_START_FORCE,
|
|
"adaptive_shear_full_force": ADAPTIVE_SHEAR_FULL_FORCE,
|
|
"adaptive_force_min": ADAPTIVE_FORCE_MIN,
|
|
"adaptive_force_max": ADAPTIVE_FORCE_MAX,
|
|
"adaptive_force_step": ADAPTIVE_FORCE_STEP,
|
|
"adaptive_force_interval": ADAPTIVE_FORCE_INTERVAL,
|
|
"release_arm_delay_seconds": RELEASE_ARM_DELAY_SECONDS,
|
|
"release_normal_change": RELEASE_NORMAL_CHANGE,
|
|
"release_shear_change": RELEASE_SHEAR_CHANGE,
|
|
"rearm_seconds": REARM_SECONDS,
|
|
"recover_normal_force": RECOVER_NORMAL_FORCE,
|
|
"recover_shear_force": RECOVER_SHEAR_FORCE,
|
|
"recover_stable_seconds": RECOVER_STABLE_SECONDS,
|
|
"open_position_recover_enabled": OPEN_POSITION_RECOVER_ENABLED,
|
|
"open_position_tolerance": OPEN_POSITION_TOLERANCE,
|
|
"open_position_stable_seconds": OPEN_POSITION_STABLE_SECONDS,
|
|
"open_position_check_interval": OPEN_POSITION_CHECK_INTERVAL,
|
|
|
|
"hold_fallback_pos": HOLD_FALLBACK_POS,
|
|
"trigger_force_update": TRIGGER_FORCE_UPDATE,
|
|
"open_at_end": OPEN_AT_END,
|
|
"dry_run": DRY_RUN,
|
|
|
|
"normal_force_calibration": NORMAL_FORCE_CALIBRATION,
|
|
"shear_force_calibration": SHEAR_FORCE_CALIBRATION,
|
|
}
|