將（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橫滾）。