orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初步完成平行夹爪旋转入孔-不断的张开转到逆时针极限再闭合转到顺时针极限

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-19 15:44:09 +08:00
parent 7af9e3d4b6
commit d8942defd1
11 changed files with 506 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/site.example.yaml
+24 -10
View File
@@ -39,7 +39,7 @@ robot:
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
tactile: true
# 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
# 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
tool_name: "tool10120938"
camera:
name: d405_left # 话题 /d405_left/... 与 91 分开
@@ -68,6 +68,8 @@ robot:
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
handeye:
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
# 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
# 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
gripper_tool: "tool10120938"
gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
@@ -177,7 +179,10 @@ robot:
# 落在本文件下方注释记录的合理区间 0.204~0.233 内。
# 复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
# 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。
# 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
# [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
# 示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
# 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
tool_tip_below_flange_m: 0.226587
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
@@ -769,17 +774,25 @@ hole_detect:
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
# 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
hole_visual_only: true
# 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
# 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
# （40mm 上限）。main 保存点是 false（多走多拍）。
hole_keypoint_once: true
# true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
# true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
hole_once_lock_xy_descend: true
# 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
# 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
# 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
once_ff_look_per_m: 0.0
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。
final_gap_mm: 2.0 # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
# 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
hole_entry_mm: 1.0 # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
# 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
# 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
# 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
# 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -792,7 +805,7 @@ hole_detect:
# ⇒ 读数往**小**错一次就永远到不了目标。
# 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
# 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
# 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
# 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
# + 任一步 _goto 走不到位就停。
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -923,8 +936,9 @@ gauge_assembly:
# ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
# 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针）
extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s]，慢一点防冲击
extreme_direction: 1 # +1=逆时针 / -1=顺时针（改符号，别去改代码）
extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
# 限位到限位约 34s（原先约 17s）
extreme_direction: -1 # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
# 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
config/site.yaml
+9 -4
View File
@@ -768,8 +768,12 @@ hole_detect:
# 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
# 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
once_ff_look_per_m: 0.0
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
# 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
final_gap_mm: 2.0 # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
# 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
hole_entry_mm: 1.0 # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
# 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
# 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
# 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
@@ -916,8 +920,9 @@ gauge_assembly:
# ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
# 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针）
extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s]，慢一点防冲击
extreme_direction: 1 # +1=逆时针 / -1=顺时针（改符号，别去改代码）
extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
# 限位到限位约 34s（原先约 17s）
extreme_direction: -1 # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
# 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"