机械臂工具坐标转换

末端姿态不对嘛？

没有搞过这方面，盲猜是坐标系不一致的问题

盲猜一个多解的问题，提示 AI 要关注机器人左右手和 J4 flag （每家叫法不一样，Epson 是叫 J4 Flag ）试试看，因为在数学上，-90°和 270°是同一个解，但是对物理设备来说，从-90°去 0.1°和从 270°去 0.1°，规划出来的路径是完全不一样的。

当然，可能需要做更多的验证，因为环节有点多

可以尝试在最终执行的时候加一个静差（比如 90°),看看是不是真的位置正确，只是旋转角度不对

@Canva 大佬您好，是的，末端姿态不对

def compute_gripper_target(
camera_data, # [xc, yc, zc, rc] 来自 VisionSystem
robot_state, # [x, y, z, r] 当前 PLC 反馈的坐标
elbow_config, # 当前机械臂的 elbow 状态
robot_joints, # [j1, j2, j3, j4] 当前关节角

# --- 标定参数 ---
camera_offset, # [dx, dy] 相机中心相对于电机中心的偏移
gripper_offset, # [dx, dy] 选定夹爪相对于电机中心的偏移
z_diff, # 夹爪指尖比相机镜头低多少 (正数)

# --- 结构参数 ---
robot_params, # {l1, l2, ...}
cam_rotation=0, # 相机安装旋转角 (0: 图像上=机器后, 90: 图像上=机器右...)
gripper_install_angle=0 # 如果夹爪本身装歪了，也可以传这个
):
try:
logger.info(f"camera data: {camera_data}")
logger.info(f"robot state: {robot_state}")
logger.info(f"elbow config: {elbow_config}")
logger.info(f"robot joints: {robot_joints}")
# 1. 解包
xc, yc, zc, rc = camera_data
curr_x, curr_y, curr_z, curr_r = robot_state
j1, j2, j3, j4 = robot_joints
cam_dx, cam_dy = camera_offset
grip_dx, grip_dy = gripper_offset

# 2. 计算当前末端绝对角度 (弧度)
# 逆时针为正
current_abs_angle_deg = j1 + j2 + j4
rad_curr = math.radians(current_abs_angle_deg)

# 3. 相机坐标 -> 法兰坐标系 (Flange Frame)
# Orbbec: X 右, Y 下. Robot: X 前, Y 左.
# 假设标准安装：相机正对下方，图像上方指向机器人后方(X-)
# 图像 X+ (右) -> 机器人 Y- (右)
# 图像 Y+ (下) -> 机器人 X- (后)

# 将角度转为弧度
# rad_cam = math.radians(cam_rotation) #

# 二维旋转公式:
# X_new = x*cos(theta) - y*sin(theta)
# Y_new = x*sin(theta) + y*cos(theta)

# 计算物料相对于相机中心(但在法兰坐标系方向下)的坐标
# x_f_rot = xc * math.cos(rad_cam) - yc * math.sin(rad_cam)
# y_f_rot = xc * math.sin(rad_cam) + yc * math.cos(rad_cam)

x_f_rot = -yc
y_f_rot = -xc


# 验证一下：
# 如果 rot=-90: cos=0, sin=-1
# x_new = 0 - y*(-1) = y (相机 Y+ 变成 法兰 X+) -> 意味着图像下方是机器人的前方
# y_new = x*(-1) + 0 = -x (相机 X+ 变成 法兰 Y-) -> 意味着图像右方是机器人的右方
# 这与你的物理描述完美契合！

# 加上物理安装偏移 (offset_x, offset_y)
obj_x_flange = x_f_rot + cam_dx
obj_y_flange = y_f_rot + cam_dy

# 4. 法兰坐标 -> 基座坐标 (保持不变)
obj_x_base = curr_x + (obj_x_flange * math.cos(rad_curr) - obj_y_flange * math.sin(rad_curr))
obj_y_base = curr_y + (obj_x_flange * math.sin(rad_curr) + obj_y_flange * math.cos(rad_curr))

# 5. 计算目标角度
# 目标是让夹爪转到 rc 角度。rc 是物料相对于相机的角度。
# 目标绝对角度 = 当前绝对角度 + rc

target_abs_angle = current_abs_angle_deg + rc

# rc = rc + cam_rotation
# phase_diff = cam_rotation - gripper_install_angle
# target_abs_angle = current_abs_angle_deg + rc + phase_diff

rad_target = math.radians(target_abs_angle)

# 6. 计算电机目标坐标
# 目标：让 "夹爪中心" 重合于 "物料中心"
# 电机坐标 = 物料坐标 - 旋转后的夹爪偏移

grip_off_x_world = grip_dx * math.cos(rad_target) - grip_dy * math.sin(rad_target)
grip_off_y_world = grip_dx * math.sin(rad_target) + grip_dy * math.cos(rad_target)

target_motor_x = obj_x_base - grip_off_x_world
target_motor_y = obj_y_base - grip_off_y_world

# 7. Z 轴计算
# zc 是相机测出的深度。如果 zc=200mm, 夹爪比相机长 50mm(z_diff=50)
# 那么还需要下降 200 - 50 = 150mm
target_motor_z = curr_z - (zc - z_diff)

# 8. 反算 PLC 需要的 R (J4 相对角)
# 调用逆解算 J1, J2
ik_res = ScaraKinematics().inverse_kinematics_v2(
target_motor_x, target_motor_y, target_motor_z, 0,
robot_params['l1'], robot_params['l2'], robot_params['z0'], robot_params['nn3'],
config_type=elbow_config
)

if not ik_res:
return None

new_j1 = ik_res['the1']
new_j2 = ik_res['the2']

# J4 = 目标绝对 - (J1 + J2)
target_motor_r = target_abs_angle - (new_j1 + new_j2)

logger.info(f">>>>>>>>>>>>>.target_motor_r: {target_motor_r}")
# 归一化
while target_motor_r > 180: target_motor_r -= 360
while target_motor_r <= -180: target_motor_r += 360

logger.info(f"target coord: {target_motor_x, target_motor_y, target_motor_z, target_motor_r}")

return [target_motor_x, target_motor_y, target_motor_z, target_motor_r]
except Exception as ex:
logger.error(f"{ex} \n{traceback.format_exc()}")
return None

这是坐标转换的代码

{
"name": "1 号夹具",
"camera": {
"offset_x": 96.0,
"offset_y": 8,
"rotation": 0
},
"main_gripper": {
"desc": "夹爪的几何中心",
"offset_x": 130.0,
"offset_y": 0.0,
"z_diff": 142,
"gripper_install_angle": -90
}
}

这是配置，rotation 和 gripper_install_angle 没用上

代码里面的相机坐标和基座标的旋转是硬编码的：

x_f_rot = -yc
y_f_rot = -xc

另外我这个 scara 的上下轴在基座上，Z+轴向上，和普通的 scara 有点不一样

从代码，似乎是通过二维定位加深度相机获取一个(XYZ)，物料位姿是通过末端与相机坐标轴对齐获取的一个姿态角实现的，没涉及三维位姿，围绕获取 rc 的相机检查一下，相机的坐标系和末端坐标系是否对齐，相机获取角度是否正确，以及可能是计算过程中变换矩阵存在的问题