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

Question

I have to make NAO robot draw some shapes on a whiteboard starting from (x, y) coordinates of each point of the shape.

So I have for example list of (x, y) coordinates of a square:

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

and now I have to give those coodinates to NAO robot in order to draw on a whiteboard. The problem is that to move the arm of NAO robot, the following example code extracted from here is given by Aldebaran (I paste here only the part of the code is important for you to understand):

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

So as you can see, I have only (x, y) coordinate of the point to be drawn on the whiteboard, but NAO robot needs much more variables to be set. I know that first I have to convert the coordinates from pixels to meters, and the conversion is the following:

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

then, seems that I have to add one dimension to the coordinates in order to get (x,y,z) coordinates.

Of course, to draw a line, the currentPos should be set as the first point and targetPos should be set as the second point. So, following the conversion:

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

and consequently:

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)

I am really not sure even if this is correct. How can I solve this problem?

Answer 1

Looking at the examples of your link, the coordinate system of your robot seems to be relative to is current bearing. So basically (1, 0, 0) points forward, (0, 1, 0) points to its left and (0, 0, 1) points up.

Now if you blackboard is on a wall (perpendicular to the ground) in front of your robot (your robot facing it), with (1, 0) pointing right and (0, 1) pointing up ("normal" cartesian system), a translation of (1, 0) on the blackboard, would be (0, -c, 0) in your robot's coordinates, and a translation of (0, 1) on the blackboard would be a translation of (0, 0, c) in your robot's coordinates, being "c" a constant for scaling. Combining both vectors, a translation of (a, b) on the blackboard, should be (0, -ac, bc) in robot space.

Hope this helps for getting started. (Not sure how the yaw, pitch and roll of your robot affects its coordinates, basically the question is whether (0, 0, 1) is perpendicular to the ground or perpendicular to pitch x roll).