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)