将（x，y）坐标转换为（x，y，z）以移动机器人手臂

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机器人的手臂，下面的示例代码从这里提取由艾尔帕兰给予（我贴在这里只代码的部分是重要的，你明白的）：

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，0，c ）在您的机器人坐标中，“ c”是用于缩放的常数。 结合两个向量，黑板上的（a，b）的翻译在机器人空间中应为（0，-ac，bc）。

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