def get_target_orientation(self):
angle = self.target_angle * np.pi / 180
mat = Gf.Matrix3f(-np.cos(angle), -np.sin(angle), 0, -np.sin(angle),
np.cos(angle), 0, 0, 0, -1)
q = mat.ExtractRotation().GetQuaternion()
(q_x, q_y, q_z) = q.GetImaginary()
q = [q_x, q_y, q_z, q.GetReal()]
return q
class TestMatrix2dEdit(_Base.TestValueEdit):
Widget = UsdQt.valueWidgets.Matrix2dEdit
SuccessValues = [Gf.Matrix2f(1.0, 2.0, 3.0, 4.0),
Gf.Matrix2d(1.0, 2.0, 3.0, 4.0)]
SuccessCastedValues = {(('1.0', '2.0'), ('3.0', '4.0')): Gf.Matrix2f(1.0, 2.0, 3.0, 4.0),
None: Gf.Matrix2d(0.0)}
KeySequences = {('1', '.', '0', QtCore.Qt.Key_Tab, '2', '.', '0', QtCore.Qt.Key_Tab,
'3', '.', '0', QtCore.Qt.Key_Tab, '4', '.', '0'): Gf.Matrix2f(1.0, 2.0, 3.0, 4.0), }
ValueErrorValues = [((1.0, None), (None, 1.0)), Gf.Matrix3f(), "(1.0, 2.0)"]
TypeErrorValues = []
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 mass_information_fn(self, prim):
massInfo = UsdPhysics.RigidBodyAPI.MassInformation()
if prim.IsA(UsdGeom.Cube):
cubeLocalToWorldTransform = UsdGeom.Xformable(
prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default())
extents = Gf.Transform(cubeLocalToWorldTransform).GetScale()
cube = UsdGeom.Cube(prim)
sizeAttr = cube.GetSizeAttr().Get()
sizeAttr = abs(sizeAttr)
extents = extents * sizeAttr
# cube volume
massInfo.volume = extents[0] * extents[1] * extents[2]
# cube inertia
inertia_diagonal = Gf.Vec3f(
1.0 / 12.0 *
(extents[1] * extents[1] + extents[2] * extents[2]), 1.0 /
12.0 * (extents[0] * extents[0] + extents[2] * extents[2]),
1.0 / 12.0 *
(extents[0] * extents[0] + extents[1] * extents[1]))
massInfo.inertia = Gf.Matrix3f(1.0)
massInfo.inertia.SetDiagonal(inertia_diagonal)
# CoM
massInfo.centerOfMass = Gf.Vec3f(0.0)
# local pose
if prim == self.rigidBodyPrim:
massInfo.localPos = Gf.Vec3f(0.0)
massInfo.localRot = Gf.Quatf(1.0)
else:
# massInfo.localPos, massInfo.localRot
lp, lr = self.get_collision_shape_local_transfrom(
cubeLocalToWorldTransform, self.rigidBodyWorldTransform)
massInfo.localPos = Gf.Vec3f(lp)
massInfo.localRot = Gf.Quatf(lr)
else:
print("UsdGeom type not supported.")
massInfo.volume = -1.0
return massInfo
def get_current_state_tr(self):
"""
Gets current End Effector Transform, converted from Motion position and Rotation matrix
"""
# Gets end effector frame
state = self.robot.end_effector.status.current_frame
orig = state["orig"] * 100.0
mat = Gf.Matrix3f(*state["axis_x"].astype(float),
*state["axis_y"].astype(float),
*state["axis_z"].astype(float))
q = mat.ExtractRotation().GetQuaternion()
(q_x, q_y, q_z) = q.GetImaginary()
q = [q_x, q_y, q_z, q.GetReal()]
tr = _dynamic_control.Transform()
tr.p = list(orig)
tr.r = q
return tr
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)
