317 lines
16 KiB
Python
317 lines
16 KiB
Python
"""真机执行层: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()
|