Files

317 lines
16 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""真机执行层:A7_Lite 单臂 + L20 手,走控制器原生 rosbridge (ws://192.168.10.21:9090)。
把这一路验证过的东西封装成可复用类:读位姿 / FK / IK(种子+绕z扫) / MoveJ(FK路径安全校验) / MoveL / 悬停 / 手命令 / 使能急停。
坐标系 base_link;euler=RPY scipy 'xyz' 弧度;speed=rad/s(0.2~0.5 慢)。仿真侧对应 grasp_exec.py 的 Sim。
⚠ 真机运动命令通常由人在终端外部运行(Claude 安全分类器会拦)。本模块可被 real_grasp.py 等调用。
用法(读):from real_arm import RealArm; a=RealArm(); print(a.read()['pos']); a.close()
"""
import time, threading
import numpy as np
import roslibpy
from scipy.spatial.transform import Rotation as Rot
class RealArm:
NS = None
def __init__(self, side="right", host="192.168.10.21", port=9090, table_z=-0.25, hand_rotation_offset_deg=0.0):
"""
Args:
side: "right" 或 "left"
host: rosbridge 地址
port: rosbridge 端口
table_z: 桌面高度(base_link 坐标系)
hand_rotation_offset_deg: 真机手安装相对仿真的额外旋转角度(度,绕法兰 z 轴)
正值=逆时针(从法兰看向手),负值=顺时针
例如:真机比仿真逆时针装了60°,传 hand_rotation_offset_deg=60.0
"""
self.side = side; self.NS = f"/{side}"; self.table = table_z
self.ros = roslibpy.Ros(host=host, port=port); self.ros.run(timeout=8)
self._st = {"m": None, "n": 0}
self._tp = roslibpy.Topic(self.ros, f"{self.NS}/arm_state", "lbot_arm_interfaces/msg/ArmState")
self._tp.subscribe(lambda m: (self._st.__setitem__("m", m), self._st.__setitem__("n", self._st["n"] + 1)))
# L20 手部控制话题(发送 20 元素 u8 位置命令)
self._hand_pos = roslibpy.Topic(self.ros, f"{self.NS}/l20hand_position_cmd", "std_msgs/UInt8MultiArray")
# 手基座到法兰的安装变换(从 scene.json 的 mount_quat_wxyz)
# 右手: [0.0, 0.7071, 0.7071, 0.0] = 绕 Y 轴旋转 90°
# 左手: [0.0, -0.7071, 0.7071, 0.0] = 绕 Y 轴旋转 -90°
if side == "right":
mount_quat_base = [0.0, 0.7071067811865475, 0.7071067811865476, 0.0]
else:
mount_quat_base = [0.0, -0.7071067811865475, 0.7071067811865476, 0.0]
# 如果真机手安装角度和仿真不一致,叠加修正旋转(绕法兰 z 轴)
if abs(hand_rotation_offset_deg) > 0.01:
from scipy.spatial.transform import Rotation as Rot
R_base = Rot.from_quat([mount_quat_base[1], mount_quat_base[2],
mount_quat_base[3], mount_quat_base[0]])
R_offset = Rot.from_euler('z', hand_rotation_offset_deg, degrees=True)
# mount = 法兰 → 手基座的旋转
# 真机手额外旋转 = 在法兰坐标系中绕 z 轴转
# 所以 mount_corrected = R_offset * mount_base
R_corrected = R_offset * R_base
q_corrected = R_corrected.as_quat() # [x,y,z,w]
self.mount_quat_wxyz = [q_corrected[3], q_corrected[0], q_corrected[1], q_corrected[2]]
print(f"[real_arm] 手安装角度修正: {hand_rotation_offset_deg}° (绕法兰 z 轴)")
else:
self.mount_quat_wxyz = mount_quat_base
self.offset_flange_to_wrist = np.array([0.0, 0.0, 0.0]) # 初始假设重合,可标定修正
def close(self):
try: self._tp.unsubscribe()
except Exception: pass
self.ros.terminate()
# ---- 服务调用 ----
def _call(self, srv, typ, arg, timeout=8.0):
# Send-layer safety gate: callers cannot bypass with verify=False.
if srv == 'movel':
raise RuntimeError('MoveL disabled: whole-arm controller path not verified')
if srv == 'movej':
import json
from pathlib import Path
from offline_table_guard import TableGuard
config=json.loads((Path(__file__).resolve().parents[1]/'config/table_safety.json').read_text())
if (self.side!='left' or config.get('validated') is not True or
config.get('movej_interpolation_validated') is not True):
raise RuntimeError('Motion blocked: tabletop/controller path validation incomplete')
guard=TableGuard(config['plane_normal_base'],config['plane_point_base_m'],config['margin_m'])
guard.check_segment(self.read(fresh=True)['joints'],arg['joints'])
cli = roslibpy.Service(self.ros, f"{self.NS}/{srv}", typ); box = {}; ev = threading.Event()
cli.call(roslibpy.ServiceRequest(arg), lambda r: (box.update(r), ev.set()), lambda e: (box.update(err=str(e)), ev.set()))
ev.wait(timeout); return box
# ---- 读位姿(强制新帧,避免掉使能陈旧值) ----
def read(self, fresh=True, timeout=2.0):
if fresh:
n0 = self._st["n"]; t0 = time.time()
while self._st["n"] == n0 and time.time() - t0 < timeout:
time.sleep(0.02)
if self._st['n']==n0:raise RuntimeError('Stale arm state; refusing motion')
m = self._st["m"]
if m is None: raise RuntimeError("收不到 arm_state——桥接/网络?")
p = m["pose"]["position"]; q = m["pose"]["orientation"]; e = m["euler"]
return {"pos": np.array([p["x"], p["y"], p["z"]]), "quat": [q["x"], q["y"], q["z"], q["w"]],
"R": Rot.from_quat([q["x"], q["y"], q["z"], q["w"]]), "euler": np.array([e["x"], e["y"], e["z"]]),
"joints": np.array([float(x) for x in m["joint_state"]["position"]])}
def fk(self, J):
r = self._call("fk", "lbot_arm_interfaces/srv/ForwardKinematics", {"joints": [float(x) for x in J]})
return np.array([r["position"][c] for c in "xyz"]) if r.get("success") else None
def ik(self, pos, R, seed=None):
"""R: scipy Rotation。返回关节角 or None。seed=当前关节 → 解相近、路径平滑。"""
fe = R.as_euler("xyz")
arg = {"position": {"x": float(pos[0]), "y": float(pos[1]), "z": float(pos[2])},
"euler": {"x": float(fe[0]), "y": float(fe[1]), "z": float(fe[2])}}
if seed is not None: arg["joints"] = [float(x) for x in seed]
r = self._call("ik", "lbot_arm_interfaces/srv/InverseKinematics", arg)
return np.array([float(x) for x in r["joints"]]) if r.get("success") else None
def ik_sweep(self, pos, R0, seed=None, zdegs=(0, -30, 30, -60, 60, -90, 90)):
"""当前朝向不可达时绕工具 z 微调找可达解(近奇异/边界常需)。返回 (joints, dz) or (None,None)。"""
for dz in zdegs:
J = self.ik(pos, R0 * Rot.from_euler("z", dz, degrees=True), seed)
if J is not None: return J, dz
return None, None
# ---- 坐标变换:手基座(wrist) ↔ 法兰(flange) ----
def wrist_to_flange(self, wrist_pos, wrist_euler):
"""手基座位姿 → 法兰位姿
仿真输出的是 hand_base_link(wrist) 位姿,真机 IK 需要法兰位姿。
变换关系:flange → hand_base_link 由 mount_quat + offset 描述。
Args:
wrist_pos: hand_base_link 位置 (3,)
wrist_euler: hand_base_link 欧拉角 xyz (3,)
Returns:
(flange_pos, flange_euler)
"""
wrist_R = Rot.from_euler('xyz', wrist_euler).as_matrix()
# mount_quat: flange → hand_base_link 的旋转
R_mount = Rot.from_quat([self.mount_quat_wxyz[1], self.mount_quat_wxyz[2],
self.mount_quat_wxyz[3], self.mount_quat_wxyz[0]]).as_matrix()
# 逆变换:hand_base_link → flange
flange_R = wrist_R @ R_mount.T
flange_pos = wrist_pos - flange_R @ self.offset_flange_to_wrist
flange_euler = Rot.from_matrix(flange_R).as_euler('xyz')
return flange_pos, flange_euler
def flange_to_wrist(self, flange_pos, flange_euler):
"""法兰位姿 → 手基座位姿
真机读取的是法兰位姿,如需转换到 hand_base_link 坐标系用此方法。
Args:
flange_pos: 法兰位置 (3,)
flange_euler: 法兰欧拉角 xyz (3,)
Returns:
(wrist_pos, wrist_euler)
"""
flange_R = Rot.from_euler('xyz', flange_euler).as_matrix()
R_mount = Rot.from_quat([self.mount_quat_wxyz[1], self.mount_quat_wxyz[2],
self.mount_quat_wxyz[3], self.mount_quat_wxyz[0]]).as_matrix()
# 正变换:flange → hand_base_link
wrist_R = flange_R @ R_mount
wrist_pos = flange_pos + flange_R @ self.offset_flange_to_wrist
wrist_euler = Rot.from_matrix(wrist_R).as_euler('xyz')
return wrist_pos, wrist_euler
def movej_wrist(self, wrist_pos, wrist_euler, speed=0.3, accel=0.3, block=False, verify=True):
"""从手基座位姿规划运动(自动转换到法兰坐标系)
用于执行 HUG 重定向输出的 hand_base_link 位姿。
内部转换为法兰位姿 → IK → movej。
Args:
wrist_pos: hand_base_link 位置 (3,)
wrist_euler: hand_base_link 欧拉角 xyz (3,)
其他参数同 movej
Returns:
success/None
"""
flange_pos, flange_euler = self.wrist_to_flange(wrist_pos, wrist_euler)
flange_R = Rot.from_euler('xyz', flange_euler)
cur = self.read()
J, dz = self.ik_sweep(flange_pos, flange_R, seed=cur["joints"])
if J is None:
print("[real_arm] movej_wrist: IK 不可达"); return None
if dz != 0:
print(f"[real_arm] movej_wrist: 绕 z 旋转 {dz}° 找到可达解")
return self.movej(J, speed=speed, accel=accel, block=block, verify=verify)
def movel_wrist(self, wrist_pos, wrist_euler, speed=0.3, accel=0.3, block=False):
"""从手基座位姿直线运动(自动转换到法兰坐标系)
笛卡尔直线到 hand_base_link 位姿,内部转换为法兰位姿后调用 movel。
近奇异慎用,优先 movej_wrist。
Args:
wrist_pos: hand_base_link 位置 (3,)
wrist_euler: hand_base_link 欧拉角 xyz (3,)
其他参数同 movel
Returns:
success/None
"""
flange_pos, flange_euler = self.wrist_to_flange(wrist_pos, wrist_euler)
flange_R = Rot.from_euler('xyz', flange_euler)
return self.movel(flange_pos, flange_R, speed=speed, accel=accel, block=block)
# ---- 路径安全(FK 逐点):不撞桌(x>0.15且z<table+margin)、不近另一臂(|y|越界) ----
def path_safe(self, J0, Jt, n=13, table_margin=0.015, y_limit=0.12):
tbad, obad = [], []
for s in np.linspace(0, 1, n):
p = self.fk((1 - s) * np.array(J0) + s * np.array(Jt))
if p is None or np.asarray(p).shape!=(3,) or not np.isfinite(p).all():
return False, ['FK failed/nonfinite; refusing path'], []
if p[0] > 0.15 and p[2] < self.table + table_margin: tbad.append((round(float(p[0]), 2), round(float(p[2]), 3)))
y = p[1] if self.side == "right" else -p[1] # 右臂防越到 +y(左臂侧),左臂对称
if y > y_limit: obad.append(round(float(p[1]), 2))
return (not tbad and not obad), tbad, obad
# ---- 运动 ----
def movej(self, Jt, speed=0.3, accel=0.3, block=False, verify=True):
"""关节空间移动到 Jt;verify=True 时先 FK 验证路径安全,不安全则拒发。返回控制器 success 或 None(拒发)。"""
if verify:
J0 = self.read()["joints"]; ok, tb, ob = self.path_safe(J0, Jt)
if not ok:
print(f"[real_arm] 拒发 MoveJ:路径不安全 撞桌{tb} 近另臂{ob}"); return None
r = self._call("movej", "lbot_arm_interfaces/srv/MoveJ",
{"joints": [float(x) for x in Jt], "speed": float(speed), "acce": float(accel), "block": bool(block)})
return r.get("success")
def movel(self, pos, R, speed=0.3, accel=0.3, block=False):
"""笛卡尔直线到 pos+R。近奇异慎用,优先 movej。"""
fe = R.as_euler("xyz")
r = self._call("movel", "lbot_arm_interfaces/srv/MoveL",
{"position": {"x": float(pos[0]), "y": float(pos[1]), "z": float(pos[2])},
"euler": {"x": float(fe[0]), "y": float(fe[1]), "z": float(fe[2])},
"speed": float(speed), "acce": float(accel), "block": bool(block)})
return r.get("success")
def move_tool_to(self, ref_pos, d, R=None, speed=0.3, block=False, verify=True):
"""把【工具参考点(法兰+偏移 d)】移到 ref_pos(base_link)、姿态 R(默认当前)。IK+MoveJ+路径校验。
d: 工具参考点相对法兰(Arm_Tip)在其坐标系的偏移(标定得到)。返回 success/None。"""
cur = self.read(); R0 = R or cur["R"]
armtip = np.asarray(ref_pos) - R0.as_matrix() @ np.asarray(d)
J, dz = self.ik_sweep(armtip, R0, seed=cur["joints"])
if J is None:
print("[real_arm] IK 不可达"); return None
return self.movej(J, speed=speed, block=block, verify=verify)
def hover_above(self, obj_top, d, clearance=0.10, R=None, speed=0.3, block=False):
"""预抓取:把工具参考点悬停在物体顶面上方 clearance 处。"""
return self.move_tool_to(np.asarray(obj_top) + np.array([0, 0, clearance]), d, R=R, speed=speed, block=block)
# ---- L20 手 ----
def set_hand_qpos(self, qpos_16):
"""设置手部关节角(仿真 16 关节弧度)→ 真机 SDK 20 u8 命令
qpos_16: 仿真 16 关节弧度数组 (thumb 4 + index 3 + middle 3 + ring 3 + pinky 3)
顺序与 l20_right_position.yml 一致
内部使用 hand_mapper 转换为 SDK 20 u8,然后通过 ROS2 话题发送。
真机控制器接收后会调用 LinkerHand SDK 的 set_joint_positions()。
"""
import sys
from pathlib import Path
sys.path.insert(0, str(Path(__file__).parent))
from hand_mapper import HandMapper
hm = HandMapper(side=self.side)
u8_cmds = hm.qpos_to_u8_commands(qpos_16)
# 发送到 ROS2 话题(std_msgs/UInt8MultiArray 格式)
self._hand_pos.publish(roslibpy.Message({
"data": [int(x) for x in u8_cmds] # 20 元素 u8 数组
}))
def set_hand_u8(self, u8_cmds_20):
"""直接发送 SDK 20 u8 命令(已映射好的情况)"""
self._hand_pos.publish(roslibpy.Message({
"data": [int(x) for x in u8_cmds_20]
}))
def set_hand_open(self):
"""张开手(所有关节归零位)"""
import numpy as np
self.set_hand_qpos(np.zeros(16))
def set_hand_closed(self, squeeze=1.0):
"""闭合手(模拟握拳)
squeeze: 闭合程度 0.0~1.0,1.0 为完全闭合
"""
import numpy as np
# 简化闭合:所有指根和指尖按比例弯曲
qpos = np.array([
# thumb: roll, yaw, pitch, mcp
0.6 * squeeze, 0.5 * squeeze, 1.0 * squeeze, 1.0 * squeeze,
# index: roll, pitch, pip
0.2 * squeeze, 1.2 * squeeze, 1.0 * squeeze,
# middle: roll, pitch, pip
0.0, 1.2 * squeeze, 1.0 * squeeze,
# ring: roll, pitch, pip
-0.2 * squeeze, 1.2 * squeeze, 1.0 * squeeze,
# pinky: roll, pitch, pip
-0.3 * squeeze, 1.2 * squeeze, 1.0 * squeeze,
])
self.set_hand_qpos(qpos)
# ---- 使能/急停(命令,慎用) ----
def enable(self, on=True):
return self._call("enable_arm", "lbot_arm_interfaces/srv/SetEnable", {"enable": bool(on)}).get("success")
def emergency(self, on=True):
return self._call("emergency_stop", "lbot_arm_interfaces/srv/SetEmergency", {"emergency": bool(on)}).get("success")
if __name__ == "__main__":
a = RealArm(); s = a.read()
print("pos:", s["pos"].round(4).tolist(), " joints:", s["joints"].round(3).tolist())
a.close()