Перевод (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 следующий пример кода, извлеченный отсюда, дан 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, 0, c)) в координатах вашего робота, являясь "c" константой для масштабирования. Сочетая оба вектора, перевод (a, b) на доске должен быть (0, -ac, bc) в пространстве робота.

Надеюсь, это поможет начать. (Не уверен, как рыскание, тангаж и крен вашего робота влияют на его координаты, в основном вопрос в том, перпендикулярен ли (0, 0, 1) земле или перпендикулярен шагу крена).