python - 将 (x, y) 坐标转换为 (x, y, z) 以移动机器人 ARM

Question

我必须让 NAO 机器人从形状每个点的 (x, y) 坐标开始在白板上绘制一些形状。

例如，我有一个正方形的 (x, y) 坐标列表:

square = [[251, 184], #point 1
          [22, 192],  #point 2
          [41, 350],  #point 3
          [244, 346]] #point 4

现在我必须将这些坐标提供给 NAO 机器人才能在白板上绘图。问题是要移动 NAO 机器人的 ARM ，以下示例代码 extracted from here由 Aldebaran 给出(我只粘贴了代码的一部分对你理解很重要):

effector   = "LArm"
space      = motion.FRAME_ROBOT
axisMask   = almath.AXIS_MASK_VEL    # just control position
isAbsolute = False              # Since we are in relative, the current position is zero
currentPos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

# Define the changes relative to the current position
dx         =  0.03      # translation axis X (meters)
dy         =  0.03      # translation axis Y (meters)
dz         =  0.00      # translation axis Z (meters)
dwx        =  0.00      # rotation axis X (radians)
dwy        =  0.00      # rotation axis Y (radians)
dwz        =  0.00      # rotation axis Z (radians)
targetPos  = [dx, dy, dz, dwx, dwy, dwz]

# Go to the target and back again
path       = [targetPos, currentPos]
times      = [2.0, 4.0] # seconds

如您所见，我只有要在白板上绘制的点的 (x, y) 坐标，但是 NAO 机器人需要设置更多的变量。我知道首先我必须将坐标从像素转换为米，转换如下:

1 pixel (px) = 1/3779.52755905511 meter [m]

然后，似乎我必须向坐标添加一维才能获得 (x,y,z) 坐标。

当然，要画一条线，currentPos应该设置为第一个点，targetPos应该设置为第二个点。因此，在转换之后:

(251, 184) pixels coordinates = (0.066, 0.047) meters coordinates
(22, 192) pixels coordinates = (0.035, 0.011) meters coordinates

因此:

currentPos = [0.066, 0.047, 0.0, 0.0, 0.0, 0.0]
dx         =  0.035      # translation axis X (meters)
dy         =  0.011      # translation axis Y (meters)
dz         =  0.00      # translation axis Z (meters)
dwx        =  0.00      # rotation axis X (radians)
dwy        =  0.00      # rotation axis Y (radians)
dwz        =  0.00      # rotation axis Z (radians)

我真的不确定这是否正确。我该如何解决这个问题？

Answer 1

查看您链接的示例，您的机器人的坐标系似乎与当前方位相关。所以基本上 (1, 0, 0) 指向前方，(0, 1, 0) 指向左侧，(0, 0, 1) 指向上方。

现在，如果你的黑板在你的机器人(你的机器人面对它)前面的墙上(垂直于地面)，(1, 0) 指向右，(0, 1) 指向上(“正常”笛卡尔系统)，黑板上 (1, 0) 的平移将是机器人坐标中的 (0, -c, 0)，黑板上 (0, 1) 的平移将是 (0, -c, 0) 0, c) 在你的机器人坐标中，“c”是一个缩放常数。结合两个向量，黑板上 (a, b) 的翻译在机器人空间中应该是 (0, -ac, bc)。

希望这有助于入门。 (不确定机器人的偏航、俯仰和滚动如何影响其坐标，基本上问题是 (0, 0, 1) 是垂直于地面还是垂直于俯仰 x 滚动)。