def ComposeRotation(rotOrder, rotVals):
'''Recomposes a rotation using the given rotation order and a list of
angles in radians.'''
twRot = Gf.Rotation(axes[rotOrder][2], Gf.RadiansToDegrees(rotVals[0]))
fbRot = Gf.Rotation(axes[rotOrder][0], Gf.RadiansToDegrees(rotVals[1]))
lrRot = Gf.Rotation(axes[rotOrder][1], Gf.RadiansToDegrees(rotVals[2]))
swRot = Gf.Rotation(axes[rotOrder][2], Gf.RadiansToDegrees(rotVals[3]))
twMat = Gf.Matrix3d(twRot)
fbMat = Gf.Matrix3d(fbRot)
lrMat = Gf.Matrix3d(lrRot)
swMat = Gf.Matrix3d(swRot)
return Gf.Matrix4d(twMat * fbMat * lrMat * swMat, Gf.Vec3d(0))
def test_GimbalLockEdgeCases(self):
'''Test the gimbal lock edge where one or more angles is 90 degrees. We
catch them all by creating rotations using all permutations of the three
rotation axes.'''
# Create all the permutations of axis switching rotation matrices.
# XXX: Note that we only use right handed matrices as the tests for
# lefthanded matrices are inconsistent right now.
mats = []
for key in axes.keys():
mat = Gf.Matrix3d(1.0)
mat.SetRow(0, axes[key][0])
mat.SetRow(1, axes[key][1])
mat.SetRow(2, axes[key][2])
if mat.IsLeftHanded():
mat = mat * -1.0
mats.append(mat)
mat = Gf.Matrix3d(1.0)
mat.SetRow(0, -axes[key][0])
mat.SetRow(1, -axes[key][1])
mat.SetRow(2, axes[key][2])
if mat.IsLeftHanded():
mat = mat * -1.0
mats.append(mat)
mat = Gf.Matrix3d(1.0)
mat.SetRow(0, -axes[key][0])
mat.SetRow(1, axes[key][1])
mat.SetRow(2, -axes[key][2])
if mat.IsLeftHanded():
mat = mat * -1.0
mats.append(mat)
mat = Gf.Matrix3d(1.0)
mat.SetRow(0, axes[key][0])
mat.SetRow(1, -axes[key][1])
mat.SetRow(2, -axes[key][2])
if mat.IsLeftHanded():
mat = mat * -1.0
mats.append(mat)
# For all our rotations, test decomposing the rotation with zeroing
# each of the angles.
for mat in mats:
rot = Gf.Matrix4d(mat, Gf.Vec3d(0))
self._TestDecomposeRotation(rot)
self._TestDecomposeRotation(rot, thetaTwHint = None)
self._TestDecomposeRotation(rot, thetaFBHint = None)
self._TestDecomposeRotation(rot, thetaLRHint = None)
self._TestDecomposeRotation(rot, thetaSwHint = None)
def test_Matrices(self):
self.TestAttrOfType(Sdf.ValueTypeNames.Matrix2d,
Sdf.ValueTypeNames.Matrix2dArray, Gf.Matrix2d(0.0), 1e-12)
self.TestAttrOfType(Sdf.ValueTypeNames.Matrix3d,
Sdf.ValueTypeNames.Matrix3dArray, Gf.Matrix3d(0.0), 1e-12)
self.TestAttrOfType(Sdf.ValueTypeNames.Matrix4d,
Sdf.ValueTypeNames.Matrix4dArray, Gf.Matrix4d(0.0), 1e-12)
def test_Exceptions(self):
# Bug USD-6284 shows that we erroneously implemented the Python 2.x
# buffer protocol 'getcharbuffer' method to expose the binary content,
# where really a string is expected. This tests that we correctly raise
# instead of treating the binary object representation as a string.
# We get different exceptions between Python 2 & 3 here, see Python
# issue 41707 (https://bugs.python.org/issue41707).
excType = TypeError if sys.version_info.major < 3 else ValueError
with self.assertRaises(excType):
int(Gf.Matrix3d(3))
with self.assertRaises(excType):
int(Gf.Matrix3f(3))
def look_at(self, gripper_pos, target):
# Y up works for look at but sometimes flips, go_local might be a safer bet with a locked y_axis
orientation = math_utils.lookAt(gripper_pos, target, (0, 1, 0))
mat = Gf.Matrix3d(orientation).GetTranspose()
self.go_local(
orig=[gripper_pos[0], gripper_pos[1], gripper_pos[2]],
axis_x=[
mat.GetColumn(0)[0],
mat.GetColumn(0)[1],
mat.GetColumn(0)[2]
],
axis_z=[
mat.GetColumn(2)[0],
mat.GetColumn(2)[1],
mat.GetColumn(2)[2]
],
)
def create_franka(self, *args):
super().create_franka()
if self.asset_path is None:
return
self.objects = [
self.asset_path +
"/Props/Flip_Stack/large_corner_bracket_physics.usd",
self.asset_path + "/Props/Flip_Stack/screw_95_physics.usd",
self.asset_path + "/Props/Flip_Stack/screw_99_physics.usd",
self.asset_path +
"/Props/Flip_Stack/small_corner_bracket_physics.usd",
self.asset_path + "/Props/Flip_Stack/t_connector_physics.usd",
]
self.current_obj = 0
# Load robot environment and set its transform
self.env_path = "/scene"
robot_usd = self.asset_path + "/Robots/Franka/franka.usd"
robot_path = "/scene/robot"
create_prim_from_usd(self._stage, robot_path, robot_usd,
Gf.Vec3d(0, 0, 0))
# Set robot end effector Target
target_path = "/scene/target"
if self._stage.GetPrimAtPath(target_path):
return
GoalPrim = self._stage.DefinePrim(target_path, "Xform")
self.default_position = _dynamic_control.Transform()
self.default_position.p = [0.4, 0.0, 0.3]
self.default_position.r = [0.0, 1.0, 0.0,
0.0] #TODO: Check values for stability
p = self.default_position.p
r = self.default_position.r
set_translate(GoalPrim, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100))
set_rotate(GoalPrim, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z)))
# Setup physics simulation
add_ground_plane(self._stage, "/groundPlane", "Z", 1000.0,
Gf.Vec3f(0.0), Gf.Vec3f(1.0))
setup_physics(self._stage)
self.add_bin()
def runTest(self):
Matrices = [(Gf.Matrix2d, Gf.Vec2d), (Gf.Matrix3d, Gf.Vec3d),
(Gf.Matrix4d, Gf.Vec4d)]
for (Matrix, Vec) in Matrices:
# constructors
self.assertIsInstance(Matrix(), Matrix)
self.assertIsInstance(Matrix(1), Matrix)
self.assertIsInstance(Matrix(Vec()), Matrix)
# python default constructor produces identity.
self.assertEqual(Matrix(), Matrix(1))
if hasNumpy:
# Verify population of numpy arrays.
emptyNumpyArray = numpy.empty(
(1, Matrix.dimension[0], Matrix.dimension[1]),
dtype='float32')
emptyNumpyArray[0] = Matrix(1)
if Matrix == Gf.Matrix2d:
self.assertIsInstance(Matrix(1, 2, 3, 4), Matrix)
self.assertEqual(Matrix().Set(1, 2, 3, 4),
Matrix(1, 2, 3, 4))
array = numpy.array(Matrix(1, 2, 3, 4))
self.assertEqual(array.shape, (2, 2))
self.assertEqual(Matrix(array), Matrix(1, 2, 3, 4))
elif Matrix == Gf.Matrix3d:
self.assertIsInstance(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9),
Matrix)
self.assertEqual(Matrix().Set(1, 2, 3, 4, 5, 6, 7, 8, 9),
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
array = numpy.array(Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
self.assertEqual(array.shape, (3, 3))
self.assertEqual(Matrix(array),
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9))
elif Matrix == Gf.Matrix4d:
self.assertIsInstance(
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16), Matrix)
self.assertEqual(
Matrix().Set(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
14, 15, 16),
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16))
array = numpy.array(
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16))
self.assertEqual(array.shape, (4, 4))
self.assertEqual(
Matrix(array),
Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16))
else:
self.fail()
self.assertEqual(Matrix().SetIdentity(), Matrix(1))
self.assertEqual(Matrix().SetZero(), Matrix(0))
self.assertEqual(Matrix().SetDiagonal(0), Matrix().SetZero())
self.assertEqual(Matrix().SetDiagonal(1), Matrix().SetIdentity())
# Test comparison of Matrix and Matrixf
#
Matrixf = {
Gf.Matrix2d: Gf.Matrix2f,
Gf.Matrix3d: Gf.Matrix3f,
Gf.Matrix4d: Gf.Matrix4f
}.get(Matrix)
size = Matrix.dimension[0] * Matrix.dimension[1]
contents = range(1, size + 1)
md = Matrix(*contents)
mf = Matrixf(*contents)
self.assertEqual(md, mf)
contents.reverse()
md.Set(*contents)
mf.Set(*contents)
self.assertEqual(md, mf)
# Convert to double precision floating point values
contents = [1.0 / x for x in contents]
mf.Set(*contents)
md.Set(*contents)
# These should *NOT* be equal due to roundoff errors in the floats.
self.assertNotEqual(md, mf)
v = Vec()
for i in range(v.dimension):
v[i] = i
m1 = Matrix().SetDiagonal(v)
m2 = Matrix(0)
for i in range(m2.dimension[0]):
m2[i, i] = i
self.assertEqual(m1, m2)
v = Vec()
for i in range(v.dimension):
v[i] = 10
self.assertEqual(Matrix().SetDiagonal(v), Matrix().SetDiagonal(10))
self.assertEqual(type(Matrix()[0]), Vec)
m = Matrix()
m[0] = makeValue(Vec, (3, 1, 4, 1))
self.assertEqual(m[0], makeValue(Vec, (3, 1, 4, 1)))
m = Matrix()
m[-1] = makeValue(Vec, (3, 1, 4, 1))
self.assertEqual(m[-1], makeValue(Vec, (3, 1, 4, 1)))
m = Matrix()
m[0, 0] = 1
m[1, 0] = 2
m[0, 1] = 3
m[1, 1] = 4
self.assertTrue(m[0, 0] == 1 and m[1, 0] == 2 and m[0, 1] == 3
and m[1, 1] == 4)
m = Matrix()
m[-1, -1] = 1
m[-2, -1] = 2
m[-1, -2] = 3
m[-2, -2] = 4
self.assertTrue(m[-1, -1] == 1 and m[-2, -1] == 2
and m[-1, -2] == 3 and m[-2, -2] == 4)
m = Matrix()
for i in range(m.dimension[0]):
for j in range(m.dimension[1]):
m[i, j] = i * m.dimension[1] + j
m = m.GetTranspose()
for i in range(m.dimension[0]):
for j in range(m.dimension[1]):
self.assertEqual(m[j, i], i * m.dimension[1] + j)
self.assertEqual(Matrix(1).GetInverse(), Matrix(1))
self.assertEqual(Matrix(4).GetInverse() * Matrix(4), Matrix(1))
self.assertNotEqual(Matrix(0).GetInverse(), Matrix(0))
self.assertEqual(
Matrix(3).GetDeterminant(), 3**Matrix.dimension[0])
self.assertEqual(len(Matrix()), Matrix.dimension[0])
# Test GetRow, GetRow3, GetColumn
m = Matrix(1)
for i in range(m.dimension[0]):
for j in range(m.dimension[1]):
m[i, j] = i * m.dimension[1] + j
for i in range(m.dimension[0]):
j0 = i * m.dimension[1]
self.assertEqual(
m.GetRow(i),
makeValue(Vec, tuple(range(j0, j0 + m.dimension[1]))))
if (Matrix == Gf.Matrix4d):
self.assertEqual(m.GetRow3(i),
Gf.Vec3d(j0, j0 + 1, j0 + 2))
for j in range(m.dimension[1]):
self.assertEqual(m.GetColumn(j),
makeValue(Vec, tuple(j+x*m.dimension[0] \
for x in range(m.dimension[0])) ))
# Test SetRow, SetRow3, SetColumn
m = Matrix(1)
for i in range(m.dimension[0]):
j0 = i * m.dimension[1]
v = makeValue(Vec, tuple(range(j0, j0 + m.dimension[1])))
m.SetRow(i, v)
self.assertEqual(v, m.GetRow(i))
m = Matrix(1)
if (Matrix == Gf.Matrix4d):
for i in range(m.dimension[0]):
j0 = i * m.dimension[1]
v = Gf.Vec3d(j0, j0 + 1, j0 + 2)
m.SetRow3(i, v)
self.assertEqual(v, m.GetRow3(i))
m = Matrix(1)
for j in range(m.dimension[0]):
v = makeValue(
Vec,
tuple(j + x * m.dimension[0]
for x in range(m.dimension[0])))
m.SetColumn(i, v)
self.assertEqual(v, m.GetColumn(i))
m = Matrix(4)
m *= Matrix(1. / 4)
self.assertEqual(m, Matrix(1))
m = Matrix(4)
self.assertEqual(m * Matrix(1. / 4), Matrix(1))
self.assertEqual(Matrix(4) * 2, Matrix(8))
self.assertEqual(2 * Matrix(4), Matrix(8))
m = Matrix(4)
m *= 2
self.assertEqual(m, Matrix(8))
m = Matrix(3)
m += Matrix(2)
self.assertEqual(m, Matrix(5))
m = Matrix(3)
m -= Matrix(2)
self.assertEqual(m, Matrix(1))
self.assertEqual(Matrix(2) + Matrix(3), Matrix(5))
self.assertEqual(Matrix(4) - Matrix(4), Matrix(0))
self.assertEqual(-Matrix(-1), Matrix(1))
self.assertEqual(
Matrix(3) / Matrix(2),
Matrix(3) * Matrix(2).GetInverse())
self.assertEqual(
Matrix(2) * makeValue(Vec, (3, 1, 4, 1)),
makeValue(Vec, (6, 2, 8, 2)))
self.assertEqual(
makeValue(Vec, (3, 1, 4, 1)) * Matrix(2),
makeValue(Vec, (6, 2, 8, 2)))
# XXX should be if scalar is double, but scalar is always double for now.
if Vec == Gf.Vec2d:
Vecf = Gf.Vec2f
elif Vec == Gf.Vec3d:
Vecf = Gf.Vec3f
elif Vec == Gf.Vec4d:
Vecf = Gf.Vec4f
self.assertEqual(
Matrix(2) * makeValue(Vecf, (3, 1, 4, 1)),
makeValue(Vecf, (6, 2, 8, 2)))
self.assertEqual(
makeValue(Vecf, (3, 1, 4, 1)) * Matrix(2),
makeValue(Vecf, (6, 2, 8, 2)))
# XXX
self.assertTrue(2 in Matrix(2) and not 4 in Matrix(2))
m = Matrix(1)
try:
m[m.dimension[0] + 1] = Vec()
except:
pass
else:
self.fail()
m = Matrix(1)
try:
m[m.dimension[0] + 1, m.dimension[1] + 1] = 10
except:
pass
else:
self.fail()
m = Matrix(1)
try:
m[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] = 3
self.fail()
except:
pass
try:
x = m[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
self.fail()
except:
pass
m = Matrix(1)
try:
m['foo'] = 3
except:
pass
else:
self.fail()
self.assertEqual(m, eval(repr(m)))
self.assertTrue(len(str(Matrix())))
def _VerifyOrthonormVecs(mat):
v0 = Gf.Vec3d(mat[0][0], mat[0][1], mat[0][2])
v1 = Gf.Vec3d(mat[1][0], mat[1][1], mat[1][2])
v2 = Gf.Vec3d(mat[2][0], mat[2][1], mat[2][2])
self.assertTrue(Gf.IsClose(Gf.Dot(v0,v1), 0, 0.0001) and \
Gf.IsClose(Gf.Dot(v0,v2), 0, 0.0001) and \
Gf.IsClose(Gf.Dot(v1,v2), 0, 0.0001))
if Matrix == Gf.Matrix3d:
m = Matrix()
m.SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30))
m2 = Matrix(m)
m_o = m.GetOrthonormalized()
self.assertEqual(m_o, m2)
m.Orthonormalize()
self.assertEqual(m, m2)
m = Matrix(3)
m_o = m.GetOrthonormalized()
# GetOrthonormalized() should not mutate m
self.assertNotEqual(m, m_o)
self.assertEqual(m_o, Matrix(1))
m.Orthonormalize()
self.assertEqual(m, Matrix(1))
m = Matrix(1, 0, 0, 1, 0, 0, 1, 0, 0)
# should print a warning
print "expect a warning about failed convergence in OrthogonalizeBasis:"
m.Orthonormalize()
m = Matrix(1, 0, 0, 1, 0, .0001, 0, 1, 0)
m_o = m.GetOrthonormalized()
_VerifyOrthonormVecs(m_o)
m.Orthonormalize()
_VerifyOrthonormVecs(m)
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
60)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 60)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
90)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 90)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
120)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
self.assertEqual(Matrix().SetScale(10), Matrix(10))
m = Matrix().SetScale(Gf.Vec3d(1, 2, 3))
self.assertTrue(m[0, 0] == 1 and m[1, 1] == 2 and m[2, 2] == 3)
# Initializing with GfRotation should give same results as SetRotate.
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
r2 = Matrix(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
self.assertEqual(
Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), 1.0)
self.assertEqual(
Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), -1.0)
self.assertEqual(
Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0).GetHandedness(), 0.0)
self.assertTrue(
Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).IsRightHanded())
self.assertTrue(
Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).IsLeftHanded())
# Test that this does not generate a nan in the angle (bug #12744)
mx = Gf.Matrix3d(0.999999999982236, -5.00662622471027e-06,
2.07636574601397e-06, 5.00666175191934e-06,
1.0000000000332, -2.19113616402155e-07,
-2.07635686422463e-06, 2.19131379884019e-07,
1)
r = mx.ExtractRotation()
# math.isnan won't be available until python 2.6
if (sys.version_info[0] >= 2 and sys.version_info[1] >= 6):
self.assertFalse(math.isnan(r.angle))
else:
# If this fails, then r.angle is Nan. Works on linux, may not be portable.
self.assertEqual(r.angle, r.angle)
if Matrix == Gf.Matrix4d:
m = Matrix()
m.SetLookAt(Gf.Vec3d(1, 0, 0), Gf.Vec3d(0, 0, 1),
Gf.Vec3d(0, 1, 0))
self.assertTrue(Gf.IsClose(m[0], Vec(-0.707107, 0, 0.707107, 0), 0.0001) and \
Gf.IsClose(m[1], Vec(0, 1, 0, 0), 0.0001) and \
Gf.IsClose(m[2], Vec(-0.707107, 0, -0.707107, 0), 0.0001) and \
Gf.IsClose(m[3], Vec(0.707107, 0, -0.707107, 1), 0.0001))
# Transform
v = Gf.Vec3f(1, 1, 1)
v2 = Matrix(3).Transform(v)
self.assertEqual(v2, Gf.Vec3f(1, 1, 1))
self.assertEqual(type(v2), Gf.Vec3f)
v = Gf.Vec3d(1, 1, 1)
v2 = Matrix(3).Transform(v)
self.assertEqual(v2, Gf.Vec3d(1, 1, 1))
self.assertEqual(type(v2), Gf.Vec3d)
# TransformDir
v = Gf.Vec3f(1, 1, 1)
v2 = Matrix(3).TransformDir(v)
self.assertEqual(v2, Gf.Vec3f(3, 3, 3))
self.assertEqual(type(v2), Gf.Vec3f)
v = Gf.Vec3d(1, 1, 1)
v2 = Matrix(3).TransformDir(v)
self.assertEqual(v2, Gf.Vec3d(3, 3, 3))
self.assertEqual(type(v2), Gf.Vec3d)
# TransformAffine
v = Gf.Vec3f(1, 1, 1)
v2 = Matrix(3).SetTranslateOnly((1, 2, 3)).TransformAffine(v)
self.assertEqual(v2, Gf.Vec3f(4, 5, 6))
self.assertEqual(type(v2), Gf.Vec3f)
v = Gf.Vec3d(1, 1, 1)
v2 = Matrix(3).SetTranslateOnly((1, 2, 3)).TransformAffine(v)
self.assertEqual(v2, Gf.Vec3d(4, 5, 6))
self.assertEqual(type(v2), Gf.Vec3d)
# Constructor, SetRotate, and SetRotateOnly w/GfRotation
m = Matrix()
r = Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)
m.SetRotate(r)
m2 = Matrix(r, Gf.Vec3d(0, 0, 0))
m_o = m.GetOrthonormalized()
self.assertEqual(m_o, m2)
m.Orthonormalize()
self.assertEqual(m, m2)
m3 = Matrix(1)
m3.SetRotateOnly(r)
self.assertEqual(m2, m3)
# Constructor, SetRotate, and SetRotateOnly w/mx
m3d = Gf.Matrix3d()
m3d.SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30))
m = Matrix(m3d, Gf.Vec3d(0, 0, 0))
m2 = Matrix()
m2 = m2.SetRotate(m3d)
m3 = Matrix()
m3 = m2.SetRotateOnly(m3d)
self.assertEqual(m, m2)
self.assertEqual(m2, m3)
m = Matrix().SetTranslate(Gf.Vec3d(12, 13, 14)) * Matrix(3)
m.Orthonormalize()
t = Matrix().SetTranslate(m.ExtractTranslation())
mnot = m * t.GetInverse()
self.assertEqual(mnot, Matrix(1))
m = Matrix()
m.SetTranslate(Gf.Vec3d(1, 2, 3))
m2 = Matrix(m)
m3 = Matrix(1)
m3.SetTranslateOnly(Gf.Vec3d(1, 2, 3))
m_o = m.GetOrthonormalized()
self.assertEqual(m_o, m2)
self.assertEqual(m_o, m3)
m.Orthonormalize()
self.assertEqual(m, m2)
self.assertEqual(m, m3)
v = Gf.Vec3d(11, 22, 33)
m = Matrix(1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
16).SetTranslateOnly(v)
self.assertEqual(m.ExtractTranslation(), v)
# Initializing with GfRotation should give same results as SetRotate
# and SetTransform
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
r2 = Matrix(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30),
Gf.Vec3d(1, 2, 3)).ExtractRotation()
r3 = Matrix().SetTransform(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30),
Gf.Vec3d(1, 2,
3)).ExtractRotation()
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
self.assertTrue(Gf.IsClose(r3.axis, r2.axis, 0.0001) and \
Gf.IsClose(r3.angle, r2.angle, 0.0001))
# Same test w/mx instead of GfRotation
mx3d = Gf.Matrix3d(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30))
r4 = Matrix().SetTransform(mx3d,
Gf.Vec3d(1, 2,
3)).ExtractRotation()
r5 = Matrix(mx3d, Gf.Vec3d(1, 2, 3)).ExtractRotation()
self.assertTrue(Gf.IsClose(r4.axis, r2.axis, 0.0001) and \
Gf.IsClose(r4.angle, r2.angle, 0.0001))
self.assertTrue(Gf.IsClose(r4.axis, r5.axis, 0.0001) and \
Gf.IsClose(r4.angle, r5.angle, 0.0001))
m4 = Matrix(mx3d, Gf.Vec3d(1, 2, 3)).ExtractRotationMatrix()
self.assertEqual(m4, mx3d)
# Initializing with GfMatrix3d
m = Matrix(1, 2, 3, 0, 4, 5, 6, 0, 7, 8, 9, 0, 10, 11, 12, 1)
m2 = Matrix(Gf.Matrix3d(1, 2, 3, 4, 5, 6, 7, 8, 9),
Gf.Vec3d(10, 11, 12))
assert (m == m2)
m = Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1)
# should print a warning
print "expect a warning about failed convergence in OrthogonalizeBasis:"
m.Orthonormalize()
m = Matrix(1, 0, 0, 0, 1, 0, .0001, 0, 0, 1, 0, 0, 0, 0, 0, 1)
m_o = m.GetOrthonormalized()
_VerifyOrthonormVecs(m_o)
m.Orthonormalize()
_VerifyOrthonormVecs(m)
m = Matrix()
m[3, 0] = 4
m[3, 1] = 5
m[3, 2] = 6
self.assertEqual(m.ExtractTranslation(), Gf.Vec3d(4, 5, 6))
self.assertEqual(
Matrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1).GetHandedness(), 1.0)
self.assertEqual(
Matrix(-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1).GetHandedness(), -1.0)
self.assertEqual(
Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0).GetHandedness(), 0.0)
self.assertTrue(
Matrix(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1).IsRightHanded())
self.assertTrue(
Matrix(-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
1).IsLeftHanded())
# RemoveScaleShear
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 0, 1), 45))
m = Gf.Matrix4d().SetScale(Gf.Vec3d(3, 4, 2)) * m
m = Gf.Matrix4d().SetRotate(Gf.Rotation(
Gf.Vec3d(1, 0, 0), -30)) * m
r = m.RemoveScaleShear()
ro = r
ro.Orthonormalize()
self.assertEqual(ro, r)
shear = Gf.Matrix4d(1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0,
0, 1)
r = shear.RemoveScaleShear()
ro = r
ro.Orthonormalize()
self.assertEqual(ro, r)
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 0, 1), 45))
m = Gf.Matrix4d().SetRotate(Gf.Rotation(
Gf.Vec3d(1, 0, 0), -30)) * m
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 1, 0),
59)) * m
r = m.RemoveScaleShear()
maxEltErr = 0
for i in range(4):
for j in range(4):
maxEltErr = max(maxEltErr, abs(r[i][j] - m[i][j]))
self.assertTrue(Gf.IsClose(maxEltErr, 0.0, 1e-5))
#
# Test Matrix4d factoring
#
def testFactor(m, expectSuccess, eps=None):
factor = lambda m: m.Factor()
if eps is not None:
factor = lambda m: m.Factor(eps)
(success, scaleOrientation, scale, rotation, \
translation, projection) = factor(m)
self.assertEqual(success, expectSuccess)
factorProduct = scaleOrientation * \
Gf.Matrix4d().SetScale(scale) * \
scaleOrientation.GetInverse() * \
rotation * \
Gf.Matrix4d().SetTranslate(translation) * \
projection
maxEltErr = 0
for i in range(4):
for j in range(4):
maxEltErr = max(maxEltErr,
abs(factorProduct[i][j] - m[i][j]))
self.assertTrue(Gf.IsClose(maxEltErr, 0.0, 1e-5))
# A rotate
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 45))
testFactor(m, True)
# A couple of rotates
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 0, 1), -45))
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 45)) * m
testFactor(m, True)
# A scale
m = Gf.Matrix4d().SetScale(Gf.Vec3d(3, 1, 4))
testFactor(m, True)
# A scale in a rotated space
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 0, 1), 45))
m = Gf.Matrix4d().SetScale(Gf.Vec3d(3, 4, 2)) * m
m = Gf.Matrix4d().SetRotate(Gf.Rotation(Gf.Vec3d(0, 0, 1), -45)) * m
testFactor(m, True)
# A nearly degenerate scale
m = Gf.Matrix4d().SetScale((Gf.MIN_VECTOR_LENGTH * 2, 1, 1))
testFactor(m, True)
# Test with epsilon.
m = Gf.Matrix4d().SetScale((Gf.MIN_VECTOR_LENGTH * 2, 1, 1))
testFactor(m, False, Gf.MIN_VECTOR_LENGTH * 3)
# A singular (1) scale in a rotated space
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(1, 1, 1), Gf.Vec3d(1, 0, 0)))
m = m * Gf.Matrix4d().SetScale(Gf.Vec3d(0, 1, 1))
m = m * Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(1, 0, 0), Gf.Vec3d(1, 1, 1)))
testFactor(m, False)
# A singular (2) scale in a rotated space
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(1, 1, 1), Gf.Vec3d(1, 0, 0)))
m = m * Gf.Matrix4d().SetScale(Gf.Vec3d(0, 0, 1))
m = m * Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(1, 0, 0), Gf.Vec3d(1, 1, 1)))
testFactor(m, False)
# A scale in a sheared space
shear = Gf.Matrix4d(1)
shear.SetRow3(0, Gf.Vec3d(1, 1, 1).GetNormalized())
m = shear.GetInverse() * Gf.Matrix4d().SetScale(Gf.Vec3d(2, 3,
4)) * shear
testFactor(m, True)
# A singular (1) scale in a sheared space
shear = Gf.Matrix4d(1)
shear.SetRow3(0, Gf.Vec3d(1, 1, 1).GetNormalized())
m = shear.GetInverse() * Gf.Matrix4d().SetScale(Gf.Vec3d(2, 0,
4)) * shear
testFactor(m, False)
# A singular (2) scale in a sheared space
shear = Gf.Matrix4d(1)
shear.SetRow3(0, Gf.Vec3d(1, 1, 1).GetNormalized())
m = shear.GetInverse() * Gf.Matrix4d().SetScale(Gf.Vec3d(0, 3,
0)) * shear
testFactor(m, False)
# A scale and a rotate
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(1, 1, -1)))
m = Gf.Matrix4d().SetScale(Gf.Vec3d(1, 2, 3)) * m
testFactor(m, True)
# A singular (1) scale and a rotate
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(1, 1, -1)))
m = Gf.Matrix4d().SetScale(Gf.Vec3d(0, 2, 3)) * m
testFactor(m, False)
# A singular (2) scale and a rotate
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(1, 1, -1)))
m = Gf.Matrix4d().SetScale(Gf.Vec3d(0, 0, 3)) * m
testFactor(m, False)
# A singular scale (1), rotate, translate
m = Gf.Matrix4d().SetTranslate(Gf.Vec3d(3, 1, 4))
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(1, 1, -1))) * m
m = Gf.Matrix4d().SetScale(Gf.Vec3d(0, 2, 3)) * m
testFactor(m, False)
# A translate, rotate, singular scale (1), translate
m = Gf.Matrix4d().SetTranslate(Gf.Vec3d(3, 1, 4))
m = Gf.Matrix4d().SetRotate(
Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(1, 1, -1))) * m
m = Gf.Matrix4d().SetScale(Gf.Vec3d(0, 2, 3)) * m
m = Gf.Matrix4d().SetTranslate(Gf.Vec3d(-10, -20, -30)) * m
testFactor(m, False)
# Test GetDeterminant and GetInverse on Matrix4d
def AssertDeterminant(m, det):
# Unfortunately, we don't have an override of Gf.IsClose
# for Gf.Matrix4d
for row1, row2 in zip(m * m.GetInverse(), Gf.Matrix4d()):
self.assertTrue(Gf.IsClose(row1, row2, 1e-13))
self.assertTrue(Gf.IsClose(m.GetDeterminant(), det, 1e-13))
m1 = Gf.Matrix4d(0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0, 1.0)
det1 = -1.0
m2 = Gf.Matrix4d(0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0)
det2 = -1.0
m3 = Gf.Matrix4d(0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 1.0, 0.0, 0.0, 0.0)
det3 = -1.0
m4 = Gf.Matrix4d(1.0, 2.0, -1.0, 3.0, 2.0, 1.0, 4.0, 5.1, 2.0, 3.0,
1.0, 6.0, 3.0, 2.0, 1.0, 1.0)
det4 = 16.8
AssertDeterminant(m1, det1)
AssertDeterminant(m2, det2)
AssertDeterminant(m3, det3)
AssertDeterminant(m4, det4)
AssertDeterminant(m1 * m1, det1 * det1)
AssertDeterminant(m1 * m4, det1 * det4)
AssertDeterminant(m1 * m3 * m4, det1 * det3 * det4)
AssertDeterminant(m1 * m3 * m4 * m2, det1 * det3 * det4 * det2)
AssertDeterminant(m2 * m3 * m4 * m2, det2 * det3 * det4 * det2)
def test_Numpy(self):
'''Converting VtArrays to numpy arrays and back.'''
try:
import numpy
except ImportError:
# If we don't have numpy, just skip this test.
return
cases = [
dict(length=33,
fill=(1, 2, 3),
arrayType=Vt.Vec3hArray,
expLen=11,
expVal=Gf.Vec3h(1, 2, 3)),
dict(length=33,
fill=(1, 2, 3),
arrayType=Vt.Vec3fArray,
expLen=11,
expVal=Gf.Vec3f(1, 2, 3)),
dict(length=33,
fill=(1, 2, 3),
arrayType=Vt.Vec3dArray,
expLen=11,
expVal=Gf.Vec3d(1, 2, 3)),
dict(length=12,
fill=(1, 2, 3, 4),
arrayType=Vt.Vec4dArray,
expLen=3,
expVal=Gf.Vec4d(1, 2, 3, 4)),
dict(length=12,
fill=(1, 2, 3, 4),
arrayType=Vt.QuatdArray,
expLen=3,
expVal=Gf.Quatd(4, (1, 2, 3))),
dict(length=12,
fill=(1, 2, 3, 4),
arrayType=Vt.QuatfArray,
expLen=3,
expVal=Gf.Quatf(4, (1, 2, 3))),
dict(length=12,
fill=(1, 2, 3, 4),
arrayType=Vt.QuathArray,
expLen=3,
expVal=Gf.Quath(4, (1, 2, 3))),
dict(length=40,
fill=(1, 0, 0, 1),
arrayType=Vt.Matrix2fArray,
expLen=10,
expVal=Gf.Matrix2f()),
dict(length=40,
fill=(1, 0, 0, 1),
arrayType=Vt.Matrix2dArray,
expLen=10,
expVal=Gf.Matrix2d()),
dict(length=90,
fill=(1, 0, 0, 0, 1, 0, 0, 0, 1),
arrayType=Vt.Matrix3fArray,
expLen=10,
expVal=Gf.Matrix3f()),
dict(length=90,
fill=(1, 0, 0, 0, 1, 0, 0, 0, 1),
arrayType=Vt.Matrix3dArray,
expLen=10,
expVal=Gf.Matrix3d()),
dict(length=160,
fill=(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1),
arrayType=Vt.Matrix4fArray,
expLen=10,
expVal=Gf.Matrix4f()),
dict(length=160,
fill=(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1),
arrayType=Vt.Matrix4dArray,
expLen=10,
expVal=Gf.Matrix4d()),
dict(length=24,
fill=(4.25, 8.5),
arrayType=Vt.Range1dArray,
expLen=12,
expVal=Gf.Range1d(4.25, 8.5)),
dict(length=24,
fill=(4.25, 8.5),
arrayType=Vt.Range1dArray,
expLen=12,
expVal=Gf.Range1d(4.25, 8.5)),
dict(
length=24,
fill=(-1, -1, 1, 1),
arrayType=Vt.Range2dArray,
expLen=6,
expVal=Gf.Range2d((-1, -1), (1, 1)),
)
]
for case in cases:
Array, length, fill, expLen, expVal = (case['arrayType'],
case['length'],
case['fill'],
case['expLen'],
case['expVal'])
src = Vt.DoubleArray(length, fill)
result = Array.FromNumpy(numpy.array(src, copy=False))
self.assertEqual(len(list(result)), expLen)
self.assertTrue(all([x == expVal for x in result]), \
'%s != %s' % (list(result), [expVal]*len(list(result))))
# Formerly failed, now produces a 1-d length-1 array.
self.assertTrue(
Vt.Vec3dArray.FromNumpy(numpy.array([1, 2, 3])) == Vt.Vec3dArray([(
1, 2, 3)]))
with self.assertRaises((ValueError, TypeError)):
# Two dimensional, but second dimension only 2 (needs 3).
a = numpy.array([[1, 2], [3, 4]])
v = Vt.Vec3dArray(a)
# Some special-case empty array cases.
self.assertEqual(numpy.array(Vt.DoubleArray()).shape, (0, ))
self.assertEqual(numpy.array(Vt.Vec4fArray()).shape, (0, 4))
self.assertEqual(numpy.array(Vt.Matrix2dArray()).shape, (0, 2, 2))
self.assertEqual(numpy.array(Vt.Matrix2fArray()).shape, (0, 2, 2))
self.assertEqual(numpy.array(Vt.Matrix3dArray()).shape, (0, 3, 3))
self.assertEqual(numpy.array(Vt.Matrix3fArray()).shape, (0, 3, 3))
self.assertEqual(numpy.array(Vt.Matrix4dArray()).shape, (0, 4, 4))
self.assertEqual(numpy.array(Vt.Matrix4fArray()).shape, (0, 4, 4))
for C in (Vt.DoubleArray, Vt.Vec4fArray, Vt.Matrix3dArray,
Vt.Matrix4dArray):
self.assertEqual(C(numpy.array(C())), C())
# Support non-contiguous numpy arrays -- slicing numpy arrays is a
# convenient way to get these.
self.assertEqual(
Vt.FloatArray.FromNumpy(
numpy.array(Vt.FloatArray(range(33)))[::4]),
Vt.FloatArray(9, (0, 4, 8, 12, 16, 20, 24, 28, 32)))
self.assertEqual(
Vt.Vec3dArray.FromNumpy(
numpy.array(Vt.Vec3fArray([(1, 2, 3), (4, 5, 6),
(7, 8, 9)]))[::-2]),
Vt.Vec3dArray([(7, 8, 9), (1, 2, 3)]))
# Check that python sequences will convert to arrays as well.
self.assertEqual(Vt.DoubleArray([1, 2, 3, 4]),
Vt.DoubleArray((1, 2, 3, 4)))
# Formerly failed, now works, producing a linear Vec4dArray.
self.assertEqual(Vt.Vec4dArray.FromNumpy(numpy.array([1, 2, 3, 4])),
Vt.Vec4dArray([(1, 2, 3, 4)]))
# Simple 1-d double array.
da = Vt.DoubleArray(10, range(10))
self.assertEqual(Vt.DoubleArray(numpy.array(da)), da)
# Vec3dArray.
va = Vt.Vec3dArray(10, [(1, 2, 3), (2, 3, 4)])
self.assertEqual(Vt.Vec3dArray.FromNumpy(numpy.array(va)), va)
def test_Matrix3Transforms(self):
Matrices = [(Gf.Matrix3d, Gf.Vec3d, Gf.Quatd),
(Gf.Matrix3f, Gf.Vec3f, Gf.Quatf)]
for (Matrix, Vec, Quat) in Matrices:
def _VerifyOrthonormVecs(mat):
v0 = Vec(mat[0][0], mat[0][1], mat[0][2])
v1 = Vec(mat[1][0], mat[1][1], mat[1][2])
v2 = Vec(mat[2][0], mat[2][1], mat[2][2])
self.assertTrue(Gf.IsClose(Gf.Dot(v0,v1), 0, 0.0001) and \
Gf.IsClose(Gf.Dot(v0,v2), 0, 0.0001) and \
Gf.IsClose(Gf.Dot(v1,v2), 0, 0.0001))
m = Matrix()
m.SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30))
m2 = Matrix(m)
m_o = m.GetOrthonormalized()
self.assertEqual(m_o, m2)
m.Orthonormalize()
self.assertEqual(m, m2)
m = Matrix(3)
m_o = m.GetOrthonormalized()
# GetOrthonormalized() should not mutate m
self.assertNotEqual(m, m_o)
self.assertEqual(m_o, Matrix(1))
m.Orthonormalize()
self.assertEqual(m, Matrix(1))
m = Matrix(1, 0, 0, 1, 0, 0, 1, 0, 0)
# should print a warning
print(
"expect a warning about failed convergence in OrthogonalizeBasis:"
)
m.Orthonormalize()
m = Matrix(1, 0, 0, 1, 0, .0001, 0, 1, 0)
m_o = m.GetOrthonormalized()
_VerifyOrthonormVecs(m_o)
m.Orthonormalize()
_VerifyOrthonormVecs(m)
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
60)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 60)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
90)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 90)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 1, 1),
120)).ExtractRotation()
r2 = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
# Setting rotation using a quaternion should give the same results
# as setting a GfRotation.
rot = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
quat = Quat(rot.GetQuaternion().GetReal(),
Vec(rot.GetQuaternion().GetImaginary()))
r = Matrix().SetRotate(rot).ExtractRotation()
r2 = Matrix().SetRotate(quat).ExtractRotation()
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
self.assertEqual(Matrix().SetScale(10), Matrix(10))
m = Matrix().SetScale(Vec(1, 2, 3))
self.assertTrue(m[0, 0] == 1 and m[1, 1] == 2 and m[2, 2] == 3)
# Initializing with GfRotation should give same results as SetRotate.
r = Matrix().SetRotate(Gf.Rotation(Gf.Vec3d(1, 0, 0),
30)).ExtractRotation()
r2 = Matrix(Gf.Rotation(Gf.Vec3d(1, 0, 0), 30)).ExtractRotation()
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
# Initializing with a quaternion should give same results as SetRotate.
rot = Gf.Rotation(Gf.Vec3d(1, 1, 1), 120)
quat = Quat(rot.GetQuaternion().GetReal(),
Vec(rot.GetQuaternion().GetImaginary()))
r = Matrix().SetRotate(quat).ExtractRotation()
r2 = Matrix(quat).ExtractRotation()
self.assertTrue(Gf.IsClose(r.axis, r2.axis, 0.0001) and \
Gf.IsClose(r.angle, r2.angle, 0.0001))
self.assertEqual(
Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), 1.0)
self.assertEqual(
Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).GetHandedness(), -1.0)
self.assertEqual(
Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0).GetHandedness(), 0.0)
self.assertTrue(Matrix(1, 0, 0, 0, 1, 0, 0, 0, 1).IsRightHanded())
self.assertTrue(Matrix(-1, 0, 0, 0, 1, 0, 0, 0, 1).IsLeftHanded())
# Test that this does not generate a nan in the angle (bug #12744)
mx = Gf.Matrix3d(0.999999999982236, -5.00662622471027e-06,
2.07636574601397e-06, 5.00666175191934e-06,
1.0000000000332, -2.19113616402155e-07,
-2.07635686422463e-06, 2.19131379884019e-07, 1)
r = mx.ExtractRotation()
# math.isnan won't be available until python 2.6
if (sys.version_info[0] >= 2 and sys.version_info[1] >= 6):
self.assertFalse(math.isnan(r.angle))
else:
# If this fails, then r.angle is Nan. Works on linux, may not be portable.
self.assertEqual(r.angle, r.angle)
def step(self, step):
if self._editor.is_playing():
if self._pending_stop:
self.stop_tasks()
return
# Updates current references and locations for the robot.
self.world.update()
self.franka_solid.update()
target = self._stage.GetPrimAtPath("/scene/target")
xform_attr = target.GetAttribute("xformOp:transform")
if self._reset:
self._paused = False
if not self._paused:
self._time += 1.0 / 60.0
self.pick_and_place.step(self._time, self._start, self._reset)
if self._reset:
# self._paused = (self._time - self._start_time) < self.timeout_max
self._paused = True
self._time = 0
self._start_time = 0
p = self.default_position.p
r = self.default_position.r
set_translate(target,
Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100))
set_rotate(target,
Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z)))
else:
state = self.franka_solid.end_effector.status.current_target
state_1 = self.pick_and_place.target_position
tr = state["orig"] * 100.0
set_translate(target, Gf.Vec3d(tr[0], tr[1], tr[2]))
set_rotate(
target,
Gf.Matrix3d(
Gf.Quatd(state_1.r.w, state_1.r.x, state_1.r.y,
state_1.r.z)))
self._start = False
self._reset = False
if self.add_objects_timeout > 0:
self.add_objects_timeout -= 1
if self.add_objects_timeout == 0:
self.create_new_objects()
# if (
# self.pick_and_place.current_state == self.current_state
# and self.current_state in [SM_states.PICKING, SM_states.GRASPING, SM_states.LIFTING]
# and (self._time - self._start_time) > self.timeout_max
# ):
# self.stop_tasks()
# elif self.pick_and_place.current_state != self.current_state:
# self._start_time = self._time
# print(self._time)
# self.current_state = self.pick_and_place.current_state
else:
translate_attr = xform_attr.Get().GetRow3(3)
rotate_x = xform_attr.Get().GetRow3(0)
rotate_y = xform_attr.Get().GetRow3(1)
rotate_z = xform_attr.Get().GetRow3(2)
orig = np.array(translate_attr) / 100.0
axis_x = np.array(rotate_x)
axis_y = np.array(rotate_y)
axis_z = np.array(rotate_z)
self.franka_solid.end_effector.go_local(
orig=orig,
axis_x=
axis_x, # TODO: consider setting this to [] for stability reasons
axis_y=axis_y,
axis_z=axis_z,
use_default_config=True,
wait_for_target=False,
wait_time=5.0,
)
"quatd": [
Gf.Quatd(0, 0, 0, 1),
[Gf.Quatd(1, 0, 0, 0),
Gf.Quatd(0, 1, 0, 0),
Gf.Quatd(0, 0, 1, 0)]
],
"matrix2d": [
Gf.Matrix2d(1, 0, 0, 0),
[
Gf.Matrix2d(0, 1, 0, 0),
Gf.Matrix2d(0, 0, 2, 0),
Gf.Matrix2d(0, 0, 3, 0)
]
],
"matrix3d": [
Gf.Matrix3d(0, 0, 0, 0, 0, 0, 0, 0, 0),
[
Gf.Matrix3d(0, 0, 0, 0, 1, 0, 0, 0, 0),
Gf.Matrix3d(0, 0, 0, 0, 0, 0, 0, 0, 2),
Gf.Matrix3d(0, 0, 4, 0, 0, 0, 0, 0, 0)
]
],
"matrix4d": [
Gf.Matrix4d(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
[
Gf.Matrix4d(1, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0),
Gf.Matrix4d(1, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0),
Gf.Matrix4d(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0)
]
],
}
def test_DecomposeWithSwingShift(self):
'''Tests decomposing into four angles using swing shift'''
# Rotation matrix without swing
rot = Gf.Matrix4d(
-0.77657773110174733, 0.41334436580878597, -0.47547183177449759, 0.0,
0.53325734733674601, 0.029348545343396093, -0.8454438268729656, 0.0,
-0.33550503583141722, -0.91010169659239692, -0.24321034680169412, 0.0,
0.0, 0.0, 0.0, 1.0)
# Additional swing rotation angle
swShift = 2.132495
# Final rotation with swing applied to the matrix
rotWithSwing = rot * \
Gf.Matrix4d(
Gf.Matrix3d(
Gf.Rotation(Gf.Vec3d(0,0,1), Gf.RadiansToDegrees(swShift))),
Gf.Vec3d(0))
# No hints
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = 300,
thetaFBHint = 300,
thetaLRHint = 300,
thetaSwHint = 300,
swShift = swShift)
for x,y in zip(result, (1.499913, -1.998063, -0.943548, -1.009098)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Hints variation 1
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = 0,
thetaFBHint = 0,
thetaLRHint = 0,
thetaSwHint = 0,
useHint = True,
swShift = swShift)
for x,y in zip(result, (1.499913, -1.998063, -0.943548, -1.009098)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Hints variation 2
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = -2.0,
thetaFBHint = -2.0,
thetaLRHint = 2.0,
thetaSwHint = -1.0,
useHint = True,
swShift = swShift)
for x,y in zip(result, (-1.641680, -1.143530, 2.198045, -1.009098)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Hints variation 3
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = -2.0,
thetaFBHint = 2.0,
thetaLRHint = 1.0,
thetaSwHint = 3.0,
useHint = True,
swShift = swShift)
for x,y in zip(result, (-1.641680, 1.998063, 0.943548, 2.132495)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Hints variation 4
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = 2.0,
thetaFBHint = 1.0,
thetaLRHint = -2.0,
thetaSwHint = 3.0,
useHint = True,
swShift = swShift)
for x,y in zip(result, (1.499913, 1.143530, -2.198045, 2.132495)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Hints variation 3
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = -2.0,
thetaFBHint = 2.0,
thetaLRHint = 1.0,
thetaSwHint = 3.0,
useHint = True,
swShift = swShift)
for x,y in zip(result, (-1.641680, 1.998063, 0.943548, 2.132495)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
# Use hints shifted by multiples of 2pi
result = \
Gf.Rotation.DecomposeRotation(rot,
twAxis = Gf.Vec3d(0, 0, 1),
fbAxis = Gf.Vec3d(1, 0, 0),
lrAxis = Gf.Vec3d(0, 1, 0),
handedness = 1,
thetaTwHint = 14.0,
thetaFBHint = 4.0,
thetaLRHint = -20.0,
thetaSwHint = -16,
useHint = True,
swShift = swShift)
for x,y in zip(result, (14.066283, 4.285122, -19.793104, -13.575468)):
self.assertAlmostEqual(x, y, places=5)
self.assertTrue(IsMatrixClose(ComposeRotation('XYZ', result), rotWithSwing))
