@
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