def test_Constructors(self):
for quatType, vec3Type, closeVal in testClasses:
self.assertIsInstance(quatType(), quatType)
self.assertIsInstance(quatType(0), quatType)
self.assertIsInstance(quatType(1, vec3Type(1, 1, 1)), quatType)
self.assertIsInstance(quatType.GetIdentity(), quatType)
q = quatType(2)
self.assertEqual(q.real, 2)
self.assertEqual(q.imaginary, vec3Type(0, 0, 0))
if quatType is not Gf.Quaternion:
# This constructor is not supported by Gf.Quaternion
q = quatType(1, 2, 3, 4)
self.assertEqual(q.real, 1)
self.assertEqual(q.imaginary, vec3Type(2, 3, 4))
q = quatType(1, vec3Type(2, 3, 4))
self.assertEqual(q.real, 1)
self.assertEqual(q.imaginary, vec3Type(2, 3, 4))
q = quatType(1, [2, 3, 4])
self.assertEqual(q.real, 1)
self.assertEqual(q.imaginary, vec3Type(2, 3, 4))
# Testing conversions between Quat[h,f,d]
self.assertIsInstance(Gf.Quath(Gf.Quatf()), Gf.Quath)
self.assertIsInstance(Gf.Quath(Gf.Quatd()), Gf.Quath)
self.assertIsInstance(Gf.Quatf(Gf.Quath()), Gf.Quatf)
self.assertIsInstance(Gf.Quatf(Gf.Quatd()), Gf.Quatf)
self.assertIsInstance(Gf.Quatd(Gf.Quath()), Gf.Quatd)
self.assertIsInstance(Gf.Quatd(Gf.Quatf()), Gf.Quatd)
def test_testQuats(self):
self.TestQuats(Sdf.ValueTypeNames.Quatd, Sdf.ValueTypeNames.QuatdArray,
Gf.Quatd(0.0), 1e-12)
self.TestQuats(Sdf.ValueTypeNames.Quatf, Sdf.ValueTypeNames.QuatfArray,
Gf.Quatf(0.0), 1e-6)
self.TestQuats(Sdf.ValueTypeNames.Quath, Sdf.ValueTypeNames.QuathArray,
Gf.Quath(0.0), 1e-2)
def test_ValueValidity(self):
validValues = [("hello", 'string'), (True, 'bool'), (1, 'uchar'),
(1, 'int'), (1, 'uint'), (1, 'int64'), (1, 'uint64'),
(1.0, 'half'), (1.0, 'float'), (1.0, 'double'),
(Gf.Vec2d(1, 2), 'double2'), (Gf.Vec2f(1, 2), 'float2'),
(Gf.Vec2h(1, 2), 'half2'), (Gf.Vec2i(1, 2), 'int2'),
(Gf.Vec3d(1, 2, 3), 'double3'),
(Gf.Vec3f(1, 2, 3), 'float3'),
(Gf.Vec3h(1, 2, 3), 'half3'), (Gf.Vec3i(1, 2,
3), 'int3'),
(Gf.Vec4d(1, 2, 3, 4), 'double4'),
(Gf.Vec4f(1, 2, 3, 4), 'float4'),
(Gf.Vec4h(1, 2, 3, 4), 'half4'),
(Gf.Vec4i(1, 2, 3, 4), 'int4'),
(Gf.Matrix4d(3), 'matrix4d'), (Gf.Quatd(1), 'quatd'),
(Gf.Quatf(2), 'quatf'), (Gf.Quath(2), 'quath'),
(Vt.StringArray(), 'string[]'),
(Vt.Vec2dArray(), 'double2[]'),
(Vt.Vec2fArray(), 'float2[]'),
(Vt.Vec2hArray(), 'half2[]'),
(Vt.Vec2iArray(), 'int2[]')]
for value, typeName in validValues:
self.assertTrue(Sdf.ValueHasValidType(value))
# Special case -- python floats will always report 'double', and
# integers will always report 'int' for the values we're testing here.
if typeName in ['half', 'float']:
self.assertEqual(Sdf.GetValueTypeNameForValue(value), 'double')
elif typeName in ['uchar', 'uint', 'int64', 'uint64']:
self.assertEqual(Sdf.GetValueTypeNameForValue(value), 'int')
else:
self.assertEqual(Sdf.GetValueTypeNameForValue(value), typeName)
self._TestRoundTrippingValue(typeName, value)
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 test_Constructors(self):
self.assertIsInstance(Gf.Quaternion(), Gf.Quaternion)
self.assertIsInstance(Gf.Quaternion(0), Gf.Quaternion)
self.assertIsInstance(Gf.Quaternion(1, Gf.Vec3d(1, 1, 1)),
Gf.Quaternion)
self.assertIsInstance(Gf.Quath(Gf.Quath()), Gf.Quath)
self.assertIsInstance(Gf.Quatf(Gf.Quatf()), Gf.Quatf)
self.assertIsInstance(Gf.Quatd(Gf.Quatd()), Gf.Quatd)
# Testing conversions between Quat[h,f,d]
self.assertIsInstance(Gf.Quath(Gf.Quatf()), Gf.Quath)
self.assertIsInstance(Gf.Quath(Gf.Quatd()), Gf.Quath)
self.assertIsInstance(Gf.Quatf(Gf.Quath()), Gf.Quatf)
self.assertIsInstance(Gf.Quatf(Gf.Quatd()), Gf.Quatf)
self.assertIsInstance(Gf.Quatd(Gf.Quath()), Gf.Quatd)
self.assertIsInstance(Gf.Quatd(Gf.Quatf()), Gf.Quatd)
def test_VaryingPrecisionOps(self):
s = Usd.Stage.CreateInMemory()
x1 = UsdGeom.Xform.Define(s, '/World')
halfRotOp = x1.AddRotateXYZOp(precision=UsdGeom.XformOp.PrecisionHalf,
opSuffix='Half')
self.assertEqual(halfRotOp.GetPrecision(), UsdGeom.XformOp.PrecisionHalf)
halfRotOp.Set(Gf.Vec3h(0.0, 0.0, 60.0))
doubleRotOp = x1.AddRotateXYZOp(precision=UsdGeom.XformOp.PrecisionDouble,
opSuffix='Double')
self.assertEqual(doubleRotOp.GetPrecision(), UsdGeom.XformOp.PrecisionDouble)
doubleRotOp.Set(Gf.Vec3d(0.0, 45.123456789, 0.0))
floatRotOp = x1.AddRotateXYZOp(opSuffix='Float')
self.assertEqual(floatRotOp.GetPrecision(), UsdGeom.XformOp.PrecisionFloat)
floatRotOp.Set(Gf.Vec3f(30.0, 0.0, 0.0))
self.assertEqual(x1.GetXformOpOrderAttr().Get(),
Vt.TokenArray(('xformOp:rotateXYZ:Half',
'xformOp:rotateXYZ:Double',
'xformOp:rotateXYZ:Float')))
xform1 = x1.GetLocalTransformation(Usd.TimeCode.Default())
x2 = UsdGeom.Xform.Define(s, '/World2')
rotOp = x2.AddRotateXYZOp(precision=UsdGeom.XformOp.PrecisionDouble).Set(Gf.Vec3d(30.0, 45.123456789, 60.0))
xform2 = x2.GetLocalTransformation(Usd.TimeCode.Default())
# Everything gets converted to double internally, so the xforms computed
# must be equal.
self.assertEqual(xform1, xform2)
x3 = UsdGeom.Xform.Define(s, '/World3')
quatd = Gf.Quatd(1., Gf.Vec3d(2., 3., 4.)).GetNormalized()
quatf = Gf.Quatf(2., Gf.Vec3f(3., 4., 5.)).GetNormalized()
quath = Gf.Quath(3., Gf.Vec3h(4., 5., 6.)).GetNormalized()
defaultOrientOp = x3.AddOrientOp().Set(quatf)
floatOrientOp = x3.AddOrientOp(precision=UsdGeom.XformOp.PrecisionFloat,
opSuffix='Float')
floatOrientOp.Set(quatf)
doubleOrientOp = x3.AddOrientOp(precision=UsdGeom.XformOp.PrecisionDouble,
opSuffix='Double')
doubleOrientOp.Set(quatd)
halfOrientOp = x3.AddOrientOp(precision=UsdGeom.XformOp.PrecisionHalf,
opSuffix='Half')
halfOrientOp.Set(quath)
# Cannot set a quatd on an op that is of precision float.
with self.assertRaises(RuntimeError):
floatOrientOp.Set(quatd)
xform3 = x3.GetLocalTransformation(Usd.TimeCode.Default())
self._AssertCloseXf(xform3,
Gf.Matrix4d(-0.27080552040616, -0.120257027227524, 0.955092988938746, 0,
0.95202181574436, 0.113459757051953, 0.284220593688289, 0,
-0.14254414216081, 0.986237867318078, 0.0837617848635551, 0,
0, 0, 0, 1))
def test_Types(self):
# Create a usda and abc temporary files.
# NOTE: This files will automatically be deleted when the test quits,
# if you want to keep them around for debugging, pass in delete=False
with tempfile.NamedTemporaryFile(suffix='.abc') as tempAbcFile, \
tempfile.NamedTemporaryFile(suffix='.usda') as tempUsdFile:
tempAbcFile.close()
tempUsdFile.close()
stage = Usd.Stage.CreateNew(tempUsdFile.name)
prim = stage.OverridePrim('/Prim')
# Note these test cases come from testSdTypes.py
usdValuesToTest = [
("hello", 'string', 'myString'),
(True, 'bool', 'myBool'),
(1, 'uchar', 'myUChar'),
(1, 'int', 'myInt'),
(1, 'uint', 'myUInt'),
(1, 'int64', 'myInt64'),
(1, 'uint64', 'myUInt64'),
(1.0, 'half', 'myHalf'),
(1.0, 'float', 'myFloat'),
(1.0, 'double', 'myDouble'),
(Gf.Vec2d(1, 2), 'double2', 'myVec2d'),
(Gf.Vec2f(1, 2), 'float2', 'myVec2f'),
(Gf.Vec2h(1, 2), 'half2', 'myVec2h'),
(Gf.Vec2i(1, 2), 'int2', 'myVec2i'),
(Gf.Vec3d(1, 2, 3), 'double3', 'myVec3d'),
(Gf.Vec3f(1, 2, 3), 'float3', 'myVec3f'),
(Gf.Vec3h(1, 2, 3), 'half3', 'myVec3h'),
(Gf.Vec3i(1, 2, 3), 'int3', 'myVec3i'),
(Gf.Vec4d(1, 2, 3, 4), 'double4', 'myVec4d'),
(Gf.Vec4f(1, 2, 3, 4), 'float4', 'myVec4f'),
(Gf.Vec4h(1, 2, 3, 4), 'half4', 'myVec4h'),
(Gf.Vec4i(1, 2, 3, 4), 'int4', 'myVec4i'),
(Gf.Matrix4d(3), 'matrix4d', 'myMatrix4d'),
(Gf.Quatf(1.0, [2.0, 3.0, 4.0]), 'quatf', 'myQuatf'),
(Gf.Quatd(1.0, [2.0, 3.0, 4.0]), 'quatd', 'myQuatd'),
(Vt.StringArray(), 'string[]', 'myStringArray'),
(Vt.Vec2dArray(), 'double2[]', 'myVec2dArray'),
(Vt.Vec2fArray(), 'float2[]', 'myVec2fArray'),
(Vt.Vec2hArray(), 'half2[]', 'myVec2hArray'),
(Vt.Vec2iArray(), 'int2[]', 'myVec2iArray'),
]
for value, typeName, attrName in usdValuesToTest:
prim.CreateAttribute(attrName,
Sdf.ValueTypeNames.Find(typeName))
prim.GetAttribute(attrName).Set(value)
stage.GetRootLayer().Save()
# Write out the USD file as .abc
UsdAbc._WriteAlembic(tempUsdFile.name, tempAbcFile.name)
# Read it back in and expect the same attributes and values
stage = Usd.Stage.Open(tempAbcFile.name)
prim = stage.GetPrimAtPath('/Prim')
self.assertTrue(prim)
for value, typeName, attrName in usdValuesToTest:
attr = prim.GetAttribute(attrName)
self.assertTrue(attr)
self.assertEqual(attr.GetTypeName(), typeName)
self.assertEqual(attr.Get(), value)
# NOTE: tempAbcFile will want to delete the underlying file
# on __exit__ from the context manager. But stage
# may have the file open. If so the deletion will
# fail on Windows. Explicitly release our reference
# to the stage to close the file.
del stage
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 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,
)
]
],
"quath": [
Gf.Quath(0, 0, 0, 1),
[Gf.Quath(1, 0, 0, 0),
Gf.Quath(0, 1, 0, 0),
Gf.Quath(0, 0, 1, 0)]
],
"quatf": [
Gf.Quatf(0, 0, 0, 1),
[Gf.Quatf(1, 0, 0, 0),
Gf.Quatf(0, 1, 0, 0),
Gf.Quatf(0, 0, 1, 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),
[
class QuatdParameter(QuatParameter):
parameterTypeString = 'quatd'
parameterWidgetString = 'vec4f'
valueTypeName = Sdf.ValueTypeNames.Quatd
_usdValueClass = Gf.Quatd
valueDefault = Gf.Quatd()