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

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滚动）。