pose_trans for a reference drive

Dieter

Hello,


I am using a UR3e and some UR_script functions. My program is supposed to drive to three positions of a Surface, calculate the normalvector (nx, ny, nz) and then calculating the rotation around x and y of normal Vector:
- Rx is calculated by Rx= -arctan(ny/nz)  
- Ry is calculated by Ry=arctan(nx/nz)
Now i want to drive to a pose "p1" relative to the calculated rotation of the global coordinate System rot_cs = p[0,0,0,Rx,Rx,0]. So I am calculating the new pose by using pose_trans(rot_cs, p1). But it's not working. I cannot find the mistake. What am I missing when I want to do the Transformation?


Thank you


Dieter

jelms

You math from normal to axis-angle format is not correct. For a normal that is in the x,y direction (0.707,0.707,0) the formulas (using atan2) give Rx = -1.57 and Ry = 1.57. The axis-angle should be Rx=-1.11, Ry = 1.11.

I think this will help. It wasn't completely clear you goal, but this shows how to take a normal vector `n` and generate a pose transformation that should takes the unit z vector to the normal vector.

def main():

    nv = p[0, 0, 0, -1.57/sqrt(2), 1.57/sqrt(2), 0]
    eig = p[0, 0, 1, 0, 0, 0]
    textmsg("a1: ", pose_trans(nv, eig))


    n = [0.0, 0.707, 0.707]
    nn = sqrt(n[0] * n[0] + n[1] * n[1])
    r = atan2(nn, n[2])
    textmsg("r: ", r)
    nv = p[0, 0, 0, -r * n[1]/nn, r * n[0]/nn, 0]
    eig = p[0, 0, 1, 0, 0, 0]
    textmsg("a2: ", pose_trans(nv, eig))

end