Files

95 lines
4.5 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.
#!/usr/bin/env python3
"""真机右臂:缓缓垂直上抬 Δz 米(姿态保持不变),MoveL 直线插值。第一次动真机用——慢速、抬升前打印计划并等你确认。
安全设计:
· 姿态用 forward_kinematics 拿当前 euler(驱动 RPY 弧度约定)原样保留,零欧拉歧义。
· 目标位置 = 当前 pose_states 位置 + (0,0,Δz);抬升前交叉校验 FK 位置≈pose_states(<2cm),不一致直接中止(防法兰/工具系错配)。
· 只发一次 MoveL、block=False;不确认(输入 y)不发指令。
· 随时急停(另开终端):
source /opt/ros/jazzy/setup.bash; source $A7_BRIDGE_SETUP
ros2 service call /robot1/right_arm/set_emergency_stop lbot_arm_interfaces/srv/SetEmergency "{emergency: true}"
运行:
source /opt/ros/jazzy/setup.bash
source $A7_BRIDGE_SETUP
python3 scripts/real_raise.py --dz 0.10 --speed 0.3
"""
import argparse, sys
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped, Vector3
from lbot_arm_interfaces.srv import ForwardKinematics, MoveL
NS = "/robot1/right_arm"
NAMES = [f"R{i}_joint" for i in range(1, 8)]
def wait_msg(node, topic, mtype, timeout=5.0):
box = {}
sub = node.create_subscription(mtype, topic, lambda m: box.setdefault("m", m), 10)
t0 = node.get_clock().now()
while rclpy.ok() and "m" not in box:
rclpy.spin_once(node, timeout_sec=0.1)
if (node.get_clock().now() - t0).nanoseconds / 1e9 > timeout:
break
node.destroy_subscription(sub)
return box.get("m")
def call(node, client, req, timeout=8.0):
if not client.wait_for_service(timeout_sec=timeout):
raise SystemExit(f"服务 {client.srv_name} 不可用")
fut = client.call_async(req)
rclpy.spin_until_future_complete(node, fut, timeout_sec=timeout)
return fut.result()
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--dz", type=float, default=0.10, help="上抬高度(米,+z=远离桌面)")
ap.add_argument("--speed", type=float, default=0.3, help="rad/s,建议 0.2~0.5")
ap.add_argument("--accel", type=float, default=0.3)
ap.add_argument("--yes", action="store_true", help="跳过确认(不建议第一次用)")
a = ap.parse_args()
rclpy.init()
node = rclpy.create_node("real_raise")
js = wait_msg(node, f"{NS}/joint_states", JointState)
ps = wait_msg(node, f"{NS}/pose_states", PoseStamped)
if js is None or ps is None:
raise SystemExit("读不到 joint_states / pose_states——确认桥接在跑、话题有数据")
jmap = dict(zip(js.name, js.position))
joints = [float(jmap[n]) for n in NAMES]
fk_cli = node.create_client(ForwardKinematics, f"{NS}/forward_kinematics")
fk = call(node, fk_cli, ForwardKinematics.Request(joints=[float(x) for x in joints]))
if not fk or not fk.success:
raise SystemExit("FK 失败")
p, e = fk.position, fk.euler
cur = ps.pose.position
# 交叉校验:FK 位置 ≈ pose_states 位置(防法兰/工具系错配)
d = ((p.x - cur.x)**2 + (p.y - cur.y)**2 + (p.z - cur.z)**2) ** 0.5
print(f"当前关节(rad): {[round(x,3) for x in joints]}")
print(f"pose_states 位置: ({cur.x:.3f},{cur.y:.3f},{cur.z:.3f})")
print(f"FK 位置: ({p.x:.3f},{p.y:.3f},{p.z:.3f}) euler(rpy rad)=({e.x:.3f},{e.y:.3f},{e.z:.3f})")
print(f"FK vs pose_states 位置差 = {d*1000:.1f} mm")
if d > 0.02:
raise SystemExit(f"⚠ FK 与 pose_states 位置差 {d*1000:.0f}mm >20mm,可能坐标系不一致——中止,不发运动指令")
tgt = Vector3(x=cur.x, y=cur.y, z=cur.z + a.dz)
print(f"\n计划: 右臂末端从 z={cur.z:.3f} 直线上抬到 z={tgt.z:.3f} (+{a.dz*100:.0f}cm, +z=远离桌面), 姿态不变")
print(f" speed={a.speed} rad/s accel={a.accel} block=False")
if not a.yes:
if input("确认发送?输入 y 回车执行,其它任意键取消: ").strip().lower() != "y":
print("已取消,未发指令。"); return
mv = node.create_client(MoveL, f"{NS}/move_linear")
req = MoveL.Request(position=tgt, euler=e, speed=float(a.speed), acce=float(a.accel), block=False)
res = call(node, mv, req)
print(f"MoveL 指令已发送,success={getattr(res,'success',None)}")
print("急停(另开终端): ros2 service call /robot1/right_arm/set_emergency_stop lbot_arm_interfaces/srv/SetEmergency \"{emergency: true}\"")
node.destroy_node(); rclpy.shutdown()
if __name__ == "__main__":
main()