Discrepancy of pose from 30002 and pendant reading

Technical Questions Robot Communication
10

In PolyScope the rotation vector is scaled so that it looks more stable and doesn’t flicker so much. The data on port 30003 is unscaled, but when scaled it corresponds to the GUI values shown in PolyScope.

Below is a Python program that exemplifies the scaling. When run it produces the following results:

PolyScope SCALED value: [0.0012193680503253582, -3.166495598686568, -0.03951768623096099]
PolyScope SCALED value: [2.4759166894662425, -5.364486160510192, 1.6506111263108283]

The first vector below “v_init” is the default startup position when the URControl is simulated. Note, the input and scaled vector are pretty similar, but not the same.

The second vector “v” is based on example data.

Notice that to calculate the scaled rotation vector, the position vector (x,y,z) is not needed - therefore it is left out of the calculation completely.

from math import *

v_init=[-0.0012, 3.1162, 0.03889]
v=[-0.06, 0.13, -0.04]

def length(v):
   return sqrt(pow(v[0],2)+pow(v[1],2)+pow(v[2],2))

def norm(v):
   l=length(v)
   norm=[v[0]/l, v[1]/l, v[2]/l]
   return norm

def _polyscope(rx,ry,rz):
   if ( (abs(rx) >= 0.001 and rx < 0.0) or (abs(rx) < 0.001 and abs(ry) >= 0.001 and ry < 0.0) or (abs(rx) < 0.001 and abs(ry) < 0.001 and rz < 0.0) ):
   scale = 1 - 2*pi / length([rx,ry,rz])
   ret = [scale*rx, scale*ry, scale*rz]
   print "PolyScope SCALED value: ", ret
   return ret
else:
   ret = [rx,ry,rz]
   print "PolyScope value: ", ret
   return ret

def polyscope(v):
   return _polyscope(v[0], v[1], v[2])

polyscope(v_init)
polyscope(v)
2 Likes