def getmanipmov(self, frPOIframe):
#How this armsegment effects the manipulator movement -relative position given in frPOIframe
#rotax to tool dist - closest point along rotax -radial distance
ori = vector3d.projection(self.rotax, frPOIframe.pos)
radialvect = frPOIframe.pos - ori
#Get tangent vector - cross prod of radial and rotax?
tangentvect = vector3d.crossproduct(self.rotax,radialvect)
return tangentvect, self.rotax
def testrot(self):
tol = 0.0000000000001
v1 = vector3d.Vector3D(*self.matrix.getcol(0))
v2 = vector3d.Vector3D(*self.matrix.getcol(1))
v3 = vector3d.Vector3D(*self.matrix.getcol(2))
n1 = vector3d.norm(v1) - 1
n2 = vector3d.norm(v2) - 1
n3 = vector3d.norm(v3) - 1
nmax = max([abs(n1), abs(n2), abs(n3)])
a1 = vector3d.anglebetween(v1, v2) - math.pi/2
a2 = vector3d.anglebetween(v1, v3) - math.pi/2
a3 = vector3d.anglebetween(v2, v3) - math.pi/2
amax = max([abs(a1), abs(a2), abs(a3)])
cr = (vector3d.crossproduct(v1, v2) - v3).norm()
if nmax > tol or amax > tol or cr > tol:
matstr = str(self.matrix) + '\n'
matstr += 'Norms: ' + str(vector3d.norm(v1)) + ' ' + \
str(vector3d.norm(v2)) + ' ' + \
str(vector3d.norm(v3)) + ' ' + '\n'
matstr += 'Angles (deg): ' + str(vector3d.anglebetween(v1, v2) *180/math.pi) + ' ' + \
str(vector3d.anglebetween(v1, v3) *180/math.pi) + ' ' + \
str(vector3d.anglebetween(v2, v3) *180/math.pi) + '\n'
matstr += str(vector3d.crossproduct(v1, v2)) + ' == ' + str(v3) + '?'
raise RuntimeError('Rotation matrix out of tolerance:\n' + matstr)
def getangax2(self):
#Axis:
#Get the eigenvector for the eigenvalue == 1:
nullspace, matrref, minv, ones, zeros = multidim.nullspace(self.matrix - multidim.identity(3), tolerance=0.00000000001)
axis = matrix2vector3D(nullspace)
axis.normalizethis()
if (axis - vector3d.xbasev()).norm() > 0.1:
v = vector3d.xbasev()
elif (axis - vector3d.ybasev()).norm() > 0.1:
v = vector3d.ybasev()
elif (axis - vector3d.zbasev()).norm() > 0.1:
v = vector3d.zbasev()
else:
raise RuntimeError('Rotation3D.getangax: Bad internal matrix!!')
v = vector3d.orthonormal(axis,v) # Make v orthonormal against axis
vrot = self.rotvec(v)
angle = vector3d.anglebetween(v, vrot)
axis1 = vector3d.crossproduct(v, vrot)
if vector3d.dotproduct(axis, axis1) < 0:
axis = -axis
return angle, axis